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PREFACE 


20 世纪 90 年 代 初 ,笔者 作为 大 一 新 生 , 参 观 了 哈尔滨 工业 大 学 机 器 人 研究 所 , 自 此 喜 
欢 上 了 机 器 人 ,并 在 这 个 领域 学 习 工作 至 今 。 机 器 人 已 由 当初 种 类 少 、 应 用 有 限 的 阶段 发 展 
到 了 现在 种 类 繁多 应 用 无 处 不 在 的 新 阶段 。 作 为 机 器 人 发 展 过 程 的 一 个 亲历 者 ,也 是 一 件 
很 值得 自豪 的 事情 。 

10 年 后 笔者 开始 为 北京 航空 航天 大 学 (北航 ) 机 器 人 研究 所 的 研究 生 主 讲 “ 机 器 人 运动 
学 "课程 ,该 课程 不 久 又 增加 了 机 器 人 动力 学 和 运动 规划 的 内 容 , 改 名 为 "机 器 人 学 ”, 笔 者 一 
直 主 讲 至 今 。 该 课程 已 由 一 门 学 院 选修 课 , 变 成 了 校 级 精品 课 ,每 年 都 有 校外 的 同学 来 选 
修 。 在 讲课 的 过 程 中 ,笔者 通过 学 习 参 考 国内 外 的 著名 教材 ,搜集 机 器 人 发 展 历程 中 的 重要 
资料 ,编写 例题 及 对 重要 理论 进行 推导 证 明 等 ,逐渐 形成 了 有 北航 特色 的 机 器 人 学 讲义 。 本 
书 就 是 在 这 本 讲义 的 基础 上 编写 而 成 的 。 笔 者 自 2015 年 开始 筹划 动笔 ,期 间 因 忙于 其 他 工 
作 导 致 编写 断断续续 ,今日 终于 成 稿 ,内 心 如 释 重负 。 

本 书 共 7 章 ,涵盖 了 机 器 人 的 发 展 简 史 、 发 展现 状 、 数 学 基础 .机 构 、 运 动 学 、 静 力学 、 动 
力学 和 运动 规划 等 机 器 人 学 的 核心 内 容 。 第 1 章 首先 对 机 器 人 的 定义 做 了 介绍 ,然后 分 古 
代 、 近 代 、 现 代 及 国内 四 部 分 介绍 了 机 器 人 的 发 展 概况 ,接着 介绍 了 机 器 人 的 基本 概念 和 分 
类 ,最 后 介绍 了 机 器 人 的 主要 研究 方向 。 第 2 章 对 机 器 人 的 机 构 做 了 介绍 ,以 机 构 的 基础 知 
识 作 为 引导 ,介绍 了 机 器 人 机 构 的 分 类 ,特点 及 机 器 人 机 构 简 图 的 画 法 ,在 此 基础 上 对 串联 
机 器 人 和 并 联机 器 人 的 典型 机 构 做 了 介绍 ,并 给 出 了 每 种 机 构 的 简 图 。 第 3 章 介绍 了 机 器 
人 的 位 姿 描 述 和 坐标 变换 ,这 是 机 器 人 学 的 数学 基础 ,首先 介绍 了 机 器 人 位 置 和 姿态 的 数学 
描述 方法 ,在 此 基础 上 介绍 了 点 的 坐标 系 平移 、 坐 标 系 旋转 及 其 综合 的 计算 方法 ,然后 介绍 
了 齐 次 坐标 、 齐 次 变换 、 旋 转变 换 通 式 及 齐 次 变换 通 式 等 内 容 , 最 后 介绍 了 机 器 人 姿态 的 
RPY 角 、 欧 拉 角 和 四 元 数 表示 方法 。 第 4 章 是 本 书 的 重点 内 容 , 首 先 介 绍 了 机 器 人 运动 学 
的 建 模 方法 ,重点 介绍 了 改进 的 D-H 建 模 方法 ,并 提供 多 个 例题 ,然后 介绍 了 机 器 人 运动 学 
逆 解 的 方法 ,包括 数值 解 方法 .解析 解 方法 及 雅 可 比方 法 ,重点 介绍 了 Paul 反 变 换 法 和 雅 可 
比 矩 阵 。 第 5 章 介绍 了 机 器 人 静 力 学 ,首先 介绍 了 机 械 臂 连 杆 受 力 与 关节 平衡 驱动 力 的 计 
算 方 法 ,然后 介绍 了 项 力 平衡 方程 . 静 力 映射 分 析 及 静 力 学 的 逆 问 题 ,最 后 介绍 了 力 与 力矩 
的 坐标 变换 方法 。 第 6 章 介绍 了 机 器 人 动力 学 ,主要 介绍 了 Lagrange 动力 学 方法 、Newton- 
Euler 动力 学 方法 及 Kane 动力 学 方法 。 第 7 章 介绍 了 机 器 人 的 运动 规划 ,首先 介绍 了 机 器 人 
的 路 径 规划 ,主要 包括 栅 格 法 的 全 局 路 径 规 划 方 法 和 势 场 法 的 局 部 路 径 规划 方法 ; 然后 介 
绍 了 机 器 人 的 轨迹 规划 ,主要 包括 关节 空间 轨迹 规划 和 操作 空间 轨迹 规划 ; 最 后 在 关节 空 
间 的 轨迹 规划 中 介绍 了 直线 插值 函数 .三 次 多 项 式 插值 函数 及 五 次 多 项 式 插值 函数 ,在 操作 
空间 的 轨迹 规划 中 主要 介绍 了 直线 轨迹 规划 和 贺 弧 轨迹 规划 方法 。 在 本 书 中 ,通过 提供 


I lll 机 器 人 学 一 一 机构、 运动 学 、 动 力学 及 运动 规划 


证 明 、 例 题 和 对 比分 析 等 方式 力求 把 知识 和 理论 介绍 清楚 ,说 明白 。 

为 了 给 本 科 生 和 研究 生 提供 一 本 简明 清楚 的 教材 或 参考 书 , 笔 考 编写 了 本 书 。 作 为 本 
科 生 的 教材 时 ,内 容 讲 到 第 4 章 即 可 ; 作为 研究 生 教材 时 ,可 通 讲 全 书 。 此 外 ,本 书 第 1 章 
含有 大 量 机 器 人 发 展 历史 的 内 容 介 绍 ,也 可 以 作为 中 小 学 生 了 解 机 器 人 的 参考 书 。 

本 书 是 在 参考 了 国内 外 著名 教材 的 基础 上 编写 而 成 ,包括 能 有 伦 院 士 的 4 机 器 人 学 》 
(1993 版 )、 蔡 自 兴 教授 的 《机 器 人 学 》( 第 2 版 ) Craig 教授 的 Introduction to Robotics : 
Mechanics and Control (Third Edition) 。 此 外 ,还 参考 .引用 了 国内 外 发 表 的 一 些 文章 、 图 

借 此 机 会 也 向 指导 和 帮助 过 笔者 的 前 辈 . 导 师 、 同 事 . 同 窗 、 好 友 表 示 感 谢 , 特 别 感 谢 笔 
者 的 博士 导师 王 树 国教 授 、. 博 士 后 导师 张 启 先 院士 。 

北航 复杂 机 构 与 智能 控制 实验 室 的 许 钦 西 、 张 印 、 田 新 扬 、 陈 翔 至 、 王 一 凡 、 史 洪 乐 等 参 
与 了 本 书 的 编写 及 校对 工作 ,在 此 深 表 感谢 。 

由 于 笔者 的 经 验 水 平 有 限 ,加 之 编写 时 间 仓 促 , 书 中 可 能 还 有 不 足 之 处 ,希望 得 到 大 家 
的 批评 指正 。 


R 强 
2019 年 1 月 
于 北京 航空 航天 大 学 
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CHAPTER 1 


机 器 人 学 是 一 门 新 兴学 科 , 仅 有 80 余年 的 发 展 历史 ,如 今 已 成 为 社会 关注 和 科学 研究 
的 热点 。 了 解 机 器 人 的 发 展 历程 及 相关 知识 对 于 学 习 机 器 人 技术 大 有 神 益 。 本 章 首先 介绍 
机 器 人 的 定义 ,以 使 读者 能 清楚 地 了 解 机 器 人 是 什么 ; 然后 介绍 机 器 人 的 发 展 历程 .机 器 人 
的 专业 术语 及 机 器 人 的 分 类 ; 最 后 介绍 机 器 人 的 主要 研究 方向 。 


1.1 机 器 人 的 定义 


目前 ,世界 上 有 各 种 各 样 的 机 器 人 ,有 地 上 跑 的 ,有 天 上 飞 的 ,有 水 中 游 的 ……, 从 太空 
到 地 面 , 从 地 面 到 水 中 ,可 以 说 机 器 人 几乎 无 处 不 在 。 然 而 ,什么 是 机 器 人 ? 或 者 机 器 人 的 
定义 是 什么 呢 ? 

“机 器 人 ?是 20 世纪 出 现 的 一 个 新 名 词 。1920 年 ,捷克 作家 卡 
勒 ， 恰 佩 克 (Karel Čapek, 1890— 1938 Æ, WR 1. 1) 发 表 了 科幻 剧 
本 《 罗 萨 姆 的 万 能 机 器 人 》(Rossum’'s Universal Robots ) ,在 剧本 中 恰 
佩 克 由 捷克 语 Robota 创造 出 了 Robot 一 词 , 意 为 “奴隶 , 苗 力 ”, 这 
就 是 “机 器 人 ”一 词 的 来 源 。 在 该 剧本 中 ,机 器 人 是 人 造 的 没有 情感 
和 思维 ,只 会 劳动 的 自动 机 器 ,但 后 来 它们 的 制造 程序 发 生 改 变 , 机 
器 人 变 得 像 人 一 样 有 了 爱 恨 情感 ,最 终 推 翻 了 人 类 ,把 人 类 当成 了 
奴隶 。 

迄今 为 止 , 不 同 的 组 织 和 学 者 为 机 器 人 给 出 了 多 个 不 同 的 
定义 。 图 1.1 Karel Čapek" 

1967 年 在 日 本 召开 的 第 一 届 机 器 人 学 术 会 议 上 ,专家 们 提出 了 
两 个 具有 代表 性 的 机 器 人 的 定义 。 一 个 是 森 政 弘 与 合 田 周 平 的 定义 , 即 “ 机 器 人 是 一 种 具有 
移动 性 、 个 体 性 、 智 能 性 、 通 用 性 、 半 机 械 半 人 性 、 自 动 性 、 奴 素性 等 7 个 特征 的 柔性 机 器 ”。 
从 这 一 定义 出 发 , 森 政 弘 又 提出 用 自动 性 、 智 能 性 、 个 体 性 、 半 机 械 半 人 性 ,作业 性 、 通 用 性 、 
信息 性 、 柔 性 有限 性 移动 性 等 10 个 特性 来 表示 机 器 人 的 形象 。 另 一 个 定义 是 加 芯 一 女 提 
出 的 具有 如 下 三 个 条 件 的 机 器 被 称 为 机 器 人 : @ 具 有 脑 、 手 、 和 脚 三 要 素 的 个 体 ; @ 具 有 非 接 
触 传感器 (用 眼 、. 耳 获取 远方 信息 ) 和 接触 传感器 ; @ 具 有 平衡 觉 和 固有 觉 的 传感器 。 

国际 标准 化 组 织 (ISO) 对 机 器 人 做 出 如 下 定义 :“ 机 器 人 是 一 种 自动 的 .位 置 可 控 的 、 具 
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有 可 编程 能 力 的 多 功能 机 械 手 ,这 种 机 械 手 有 几 个 轴 , 能 够 借助 可 编程 操作 来 处 理 各 种 材 
料 .零件 .工具 和 专用 装置 ,以 执行 各 种 任务 ”。 ` 

美国 国家 标准 局 (ANSID 对 机 器 人 的 定义 是 “一 种 能 够 进行 编程 并 在 自动 控制 下 执行 某 
些 操作 和 移动 作业 任务 的 机 械 装置 ”。 

《大 英 百 科 全 书 》 中 关于 机 器 人 的 定义 是 “机 器 人 是 一 种 可 取代 人 工 的 自动 操作 机 器 , 尽 
管 其 外 表 不 像 人 或 不 能 像 人 那样 执行 任务 (Any automatically operated machine that 
replaces human effort,though it may not resemble human beings in appearance or perform 
functions in a humanlike manner—— Encyclopaedia Britannica)”, 

日 本 机 器 人 协会 将 机 器 人 定义 为 "一 种 具有 感觉 和 识别 能 力 , 并 能 够 控制 自身 行为 的 
机 器 ”。 

中 国有 科学 家 对 机 器 人 的 定义 是 “机 器 人 是 一 种 自动 化 的 机 器 ,所 不 同 的 是 这 种 机 器 具 
备 一 些 与 人 或 生物 相似 的 智能 能 力 ,如 感知 能 力 ,规划 能 力 ,动作 能 力 和 协同 能 力 , 是 一 种 具 
有 高 度 灵活 性 的 自动 化 机 器 ”。 

随 着 机 器 人 技术 的 飞速 发 展 , 机 器 人 不 断 向 各 个 领域 拓展 ,机 器 人 从 外 观 上 已 脱离 了 最 
初 仿 人 型 机 器 人 和 工业 机 器 人 所 具有 的 形状 ,其 功能 和 智能 化 程度 也 大 大 增强 。 现 在 来 看 ， 
过 去 的 机 器 人 定义 一 般 只 能 描述 一 类 或 几 类 机 器 人 ,难以 对 所 有 类 型 的 机 器 人 进行 准确 定 
义 , 因 此 在 新 的 时 代 , 机 器 人 的 定义 也 需要 不 断 地 充实 和 完善 。 

为 了 便于 理解 ,我 们 在 此 给 出 一 个 简单 好 记 的 机 器 人 定义 : 机 器 人 是 一 种 高 度 自 动 化 、 


需要 说 明 的 是 ,有 些 软件 程序 也 被 称 为 "机 器 人 (Robot/Bot)”, 是 指 某 些 能 不 间断 地 执 
行 信息 搜索 比较 任务 的 软件 程序 ,主要 用 于 搜索 引擎 。 

关于 机 器 人 没有 统一 定义 的 问题 ,读者 们 大 可 不 必 为 此 担忧 。 众 所 周知 ,机 器 人 一 词 最 
早 诞生 于 科幻 剧本 中 , 它 是 人 们 结合 自身 提出 的 一 种 充满 幻想 的 对 象 ,而 这 种 科幻 对 象 往往 
缺乏 具象 ,其 描述 上 的 弹性 特别 大 ,因此 是 很 难 完整 定义 和 描述 的 。 此 外 ,现实 中 的 机 器 人 
又 随 着 科技 的 发 展 或 新 技术 的 出 现 , 不 断 涌现 出 新 的 结构 、 新 的 功能 和 新 的 类 型 ,而 有 些 科 
技 又 是 跨 时 代 的 ,因此 以 前 的 机 器 人 定义 很 难 描述 新 机 器 人 的 特征 。 也 许 机 器 人 永远 不 会 
有 一 个 统一 的 定义 ,但 正 是 如 此 , 才 说 明了 机 器 人 技术 有 无 限 的 生命 力 和 不 断 进步 发 展 的 
空间 。 

工业 机 器 人 是 目前 应 用 最 广泛 的 机 器 人 ,下 面 给 两 个 工业 机 器 人 的 定义 ,便于 读者 们 
理解 。 

(1) 工业 机 器 人 是 一 种 设计 用 来 搬运 材料 、 零 件 . 工 具 或 特殊 设备 的 可 再 编程 的 多 功能 
操作 臂 , 它 能 通过 可 变 的 编程 运动 执行 各 种 任务 (Industrial Robot is a re-programmable， 
multifunctional manipulator designed to move materials, parts, tools,or specialized devices 
through variable programmed motions to perform a variety of tasks). 

(2) ISO 定义 工业 机 器 人 是 一 种 自动 控制 的 .可 再 编程 的 多 用 途 操 作 臂 ,一 般 有 三 个 或 
以 上 的 可 编程 关节 (An industrial robot is defined by ISO as an automatically controlled, 


reprogrammable, multipurpose manipulator programmable in three or more axes), 
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1.2 机 器 人 的 发 展 简介 


本 节 主 要 从 古代 关于 机 器 人 的 记载 .近代 机 器 人 的 发 展 、. 现 代 机 器 人 的 发 展 以 及 我 国 机 
器 人 的 发 展 这 四 个 方面 来 介绍 机 器 人 的 发 展 历程 。 


1.2.1 古代 关于 机 器 人 的 记载 


我 国 古 代 就 有 关于 机 器 人 或 类 机 器 人 的 记载 。 据 《列子 ， 汤 问 》 记 载 , 西 周 穆王 时 期 ( 公 
元 前 1023 一 957 年 ) ,能 工 巧 匠 优 师 就 用 动物 毛皮 .木头 、 树 脂 .颜料 制作 出 了 酷似 真人 的 木 
偶 , 能 歌 善 舞 “千变万化 , 惟 意 所 适 。 王 以 为 实 人 也 ”, 这 是 我 国 最 早 关于 机 器 人 的 记载 。 

据 《 墨 经 》 记 载 ,春秋 后 期 (公元 前 507 一 444 年 ) ,我 国 著名 的 木匠 鲁班 制造 了 一 只 木 鸟 ， 
能 在 空中 飞行 “三 日 不 下 ”。 

东汉 时 期 ,科学 家 张衡 (公元 78 一 139 年 ) 发 明了 计 里 鼓 车 和 指南 车 。 如 图 1.2 所 示 , 计 
里 鼓 车 每 行 一 里 ,车 上 木 人 击 鼓 一 下 ,每 行 十 里 击 钟 一 下 。 指 南车 具有 复杂 轮 系 装置 , 若 车 
上 木 人 运动 起 始 指 向 南方 , 则 该 车 无 论 如 何 运 动 ,其 指向 始终 不 变 , 与 指南 针 有 异曲同工 
之 妙 。 

三 国 时 期 , 建 兴 九 年 至 十 二 年 (公元 231 一 234 年 ) , 蜀 国 丞相 诸葛 亮 北伐 中 原 时 ,为 解决 
山路 崎 虹 .运输 军粮 困难 的 问题 ,创造 出 了 “ 木 牛 流 马 ?。 木 牛 流 马 不 用 吃 草 , 其 载重 为 "一 岁 
粮 ”( 约 四 百 斤 左右 ) ,每 日 行程 为 “ 特 行者 数 十 里 , 群 行 三 十 里 ,为 蜀 国 十 万 大 军 运 送 粮食 。 

在 国外 ,公元 前 2 世纪 ,亚历山大 时 代 的 古 希 腊 人 发 明了 最 原始 的 机 器 人 : 一 个 以 水 、 
空气 和 藻 汽 压力 为 动力 的 会 动 的 有 雕像 , 它 可 以 自己 开门 ,还 可 以 借助 蒸汽 唱歌 。 

1640 年 ,意大利 人 莱 昂 纳 多 。 达 芬 奇 (Leonardo DaVinci) 设计 了 一 个 铠甲 武士 ,如 
图 1. 3 所 示 。 该 铠甲 武士 用 此 轮 和 连 杆 作为 传动 装置 ,能 够 像 人 一 样 做 各 种 动作 ,如 挥动 胎 
膊 、 坐 下 .起立 .转动 头 部 、 开 合 下 颌 等 。 


图 1.2 计 里 鼓 车 中 图 1.3 铠甲 武士 六 
1662 年 ,日 本 的 竹田 近江 利用 钟表 技术 发 明了 自动 机 需 玩 偶 , 并 在 大 孤 的 道 顿 亏 演 出 。 
1738 年 ,法 国 天 才 技师 杰克 “。 戴 ， 瓦 克 逊 发 明 一 只 机 器 了 鸭 , 能 完成 吸食 鸣叫、 游泳 等 
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动作 。 

1770 年 ,瑞士 钟表 匠 雅 克 。 德 罗 (Jaquet Droz) 父 \ 子 制作 了 会 写字 绘画、 弹琴 的 三 种 人 
偶 , 如 图 1.4 所 示 ， ei es 用 齿轮 传动 和 发 条 驱动 原理 制 成 的 ,制作 精巧 ,动作 逼真 ， 
宛如 真人 一 般 , 在 欧洲 风靡 一 时 。 

1770 年 ,英国 著名 钟表 商 Williamson 向 清朝 乾隆 皇帝 进贡 了 一 个 铜 镀金 写字 人 钟 ， 
高 231 厘米 , 钟 内 有 一 个 写字 人 偶 , 如 图 1.5 所 示 , 该 人 偶 能 用 毛笔 写 出 “八方 向 化 , 九 土 
来 王 ? 的 对 联 , 且 写字 时 人 偶 头 部 随 之 摆动 , 棚 棚 如 生 。 目 前 该 写字 人 钟 存放 于 故宫 钟 
表 馆 。 


图 1.4 三 种 人 偶 ” 图 1.5 铜 镀金 写字 人 钟 6 


1.2.2 近代 机 器 人 的 发 展 


机 器 人 的 研究 始 于 20 世纪 中 期 ,其 技术 背景 是 计算 机 技术 .控制 理论 .自动 化 技术 的 快 
速 发 展 以 及 原子 能 的 开发 利用 。 

1939 年 ,美国 纽约 世博 会 上 展 出 了 西屋 电气 公司 制造 的 家 用 机 器 人 Elektro, É HH E 
供电 :可 以 行走 ,会 说 77 个 字 , 甚 至 可 以 抽烟 ,虽然 离 真正 干 家 务 还 差 得 很 远 , 但 它 让 人 们 对 
机 器 人 的 认识 变 得 更 加 具体 。 随 着 20 世纪 科技 的 飞速 发 展 ,西方 发 达 国 家 感觉 恰 佩 克 描 述 
的 机 器 人 很 快 就 会 出 现 , 因 此 对 机 器 人 产生 了 极 大 的 恐惧 和 不 安 。 

1942 年 ,科幻 作家 阿 西 莫 夫 (lsaac Asimov, 1920—1992 年 ， 
参见 图 1. 6) 在 其 科幻 小 说 (Runaround》 中 提出 了 著名 的 “机 器 人 
学 三 原则 (The Three laws of Robotics)”: 

第 一 、 机 器 人 不 得 危害 人 类 ,也 不 能 在 人 类 受伤 害 时 袖 手 旁 
观 (A robot may not injure a human being or,through inaction, 
allow a human being to come to harm); 

第 二 、 机 器 人 必须 服从 人 类 的 命令 ,但 命令 违反 第 一 条 原则 


时 例外 (A robot must obey the orders given it # human beings 


except where such orders would conflict with the First Law); 
第 三 .在 不 违反 第 一 条 和 第 二 条 原则 时 ,机 器 人 必须 保护 自 


身 不 受伤 害 (A robot must protect its own existence as long as such protection does not 


图 1.6 Isaac Asimov!° 


conflict with the First or Second Law), 


上 述 三 原则 虽然 只 是 科幻 小 说 中 的 创造 ,但 却 成 为 机 器 人 研究 的 伦理 性 纲领 ,机 器 人 学 
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术 界 一 直 将 这 三 原则 作为 机 器 人 开发 的 准则 。 

Á 1946 年 世界 上 第 一 台 通 用 计算 机 ENIAC 问世 以 来 ,计算 机 技术 取得 了 惊人 的 进 
步 ,向 高 速度 、 大 容量 、 低 价格 的 方向 飞快 发 展 。1948 年 , 诺 伯 特 ， 维 纳 (Norbert Wiener) E 
版 了 《控制 论 X《Cybernetics) ,阐述 了 机 器 中 的 通信 和 控制 机 能 与 人 的 神经 ,感觉 机 能 的 共同 
规律 ,率先 提出 了 以 计算 机 为 核心 的 自动 化 工厂 的 概念 。 同 时 ,制造 业 大 批量 生产 的 迫切 需 
求 推动 了 自动 化 技术 的 发 展 ,其 结果 之 一 便 是 1952 年 MIT 研制 出 世界 首 台 数控 机 床 。 与 
数控 机 床 相关 的 控制 .机 械 零件 的 研究 又 为 机 器 人 的 开发 莫 定 了 基础 。 

tt 需要 某 些 具有 操作 功能 的 机 械 代 替 人 处 理 屏 蔽 室内 的 放射 性 

。 在 这 一 需求 背景 下 ,美国 的 橡树 岭 国家 实验 室 (Oak-Ridge National Laboratory) 与 阿 
尔 贡 o is National Laboratory) F 1947 年 开发 了 遥控 机 械 手 ,1948 年 又 开 
发 了 机 械 式 的 主 从 机 械 手 。 主 从 机 械 手 由 同 构 的 两 个 机 械 手 组 成 , 主 手 和 从 手 由 机 械 传动 
装置 连接 , 主 手 由 人 操控 ,从 手 能 同步 完成 主 手相 同 的 运动 和 操作 。 

1954 年 ,美国 发 明 家 戴 沃 (George Charles Devol, 1912 
2011 年 , 见 图 1.7) 提 出 了 工业 机 需 人 的 概念 稚 形 Programmed 
Article Transfer ,命名 为 Unimate, 意思 为 通用 自动 化 (Universal 
Automation) ,并 申请 了 专利 (U. S. Patent 2988237)。 估 计 是 
该 专利 涉及 的 技术 过 于 超前 ,直到 1961 年 才 被 授权 。 

1955 年 ,J. Denavit 和 R. S. Hartenberg 联合 在 《Applied 
Mechanics》 期 刊 上 发 表 了 一 篇 名 为 A kinematic notation for 
low-pair mechanisms based on matrices 的 论文 ,用 齐 次 变换 和 矩 
阵 描 述 两 个 相 邻 连 杆 坐标 系 间 的 空间 为 工业 机 器 人 
的 运动 学 建 模 提供 了 重要 的 理论 基础 ,迄今 该 方法 仍然 被 广泛 。 图 1.7 George C. Devol 
采用 。 

1959 年 , 戴 沃 与 合 估 人 约瑟夫 。 恩 格 尔 伯 格 (Joseph Engelberger, 1925—2015 年 , 见 
图 1. 8) 成 立 了 世界 上 第 一 家 制造 机 器 人 的 公司 一 一 Unimation , 并 sue 出 世界 上 第 一 
台 工 业 机 器 人 Unimate, 如 图 1. 9 所 示 , 开 创 了 机 器 人 发 展 的 新 纪元 。 由 于 恩格尔 伯 格 在 工 
业 机 器 人 的 研发 和 宣传 方面 的 巨大 贡献 ,他 被 称 为 “工业 机 器 人 之 父 ”, 虽 然 世 界 上 第 一 台 工 
业 机 器 人 的 发 明 人 是 戴 沃 。Unimate 机 器 人 只 有 5 + B H J ,采用 真空 管控 制 液压 驱动 。 


图 1.8 Joseph Engelberger™ 图 1.9 Unimate 机 器 人 "” 
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1961 年 ,Unimate 机 器 人 被 用 在 了 通用 汽车 在 新 泽 西 州 的 Inland Fisher Guide 工厂 ， 
用 于 将 铸造 的 汽车 门 把 手 等 热 的 铸件 放 和 冷却 池 中 ,如 图 1. 10 所 示 > 从 而 将 工人 从 恶劣 的 
工作 环境 中 解放 出 来 。 

1962 年 ,美国 机 械 与 铸造 公司 (American Machine and Foundry) 推 出 第 一 台 圆 柱 坐 标 
HLAFA Verstran, 意 思 是 “万 能 搬运 ”, 如 图 1. 11 所 示 。 同 一 年 ,6 台 Verstran 搬运 机 絮 人 被 
应 用 于 美国 坎 顿 (Canton) 的 福特 汽车 制造 厂 。 该 机 器 人 也 与 Unimate 机 器 人 一 样 成 为 商 
业 化 的 工业 机 器 人 ,出 口 到 世界 各 国 。 


图 1.10 Unimate 机 器 人 在 通用 汽车 工厂 中 图 1.11 Verstran 圆柱 坐标 机 器 人 050 


20 世纪 60 年 代 中 期 开始 ,美国 麻 省 理工 学 院 (MIT)、 斯 坦 福 大 学 、 英 国 爱丁堡 大 学 等 
陆续 成 立 了 机 器 人 实验 室 。 美 国 开始 研究 第 二 代 带 传感器 、 有 感知 的 机 器 人 ,并 向 人 工 智能 
领域 进军 。 

1965 年 ,MIT 的 L. G. Roberts 演示 了 第 一 个 具有 视觉 传感器 、 能 识别 与 定位 简单 积木 
的 机 器 人 系统 ,他 通过 计算 机 程序 从 数字 图 像 中 提取 出 诸如 立方 体 、 枫 形体 .棱柱 体 等 多 面 
体 的 三 维 结构 ,并 对 物体 形状 及 物体 的 空间 关系 进行 描述 。Roberts 的 研究 工作 开创 了 以 
了 解 三 维 场景 为 目的 的 三 维 立体 机 器 视觉 的 研究 。 

1967 年 ,日 本 成 立 了 专门 的 机 器 人 研究 学 会 一 一 人 工 手 研究 会 , 现 改名 为 仿生 机 构 研 
究 会 ,同年 召开 了 日 本 首届 机 器 人 学 术 会 议 。 

" 1968 年 ,美国 斯 坦 福 研 究 所 (Stanford Research 
Institute) 研 发 出 机 器 人 Shakey, 这 是 世界 上 首 台 
采用 了 人 工 智能 的 移动 机 器 人 ,如 图 1. 12 所 示 。 
它 安 装 了 了 摄像机、 三角 测 距 仪 碰撞 传感器 等 , 通 
过 无 线 系统 由 两 台 计 算 机 控制 ,能 够 自主 进行 感 
知 、 环 境 建 模 、 行 为 规划 ,能 自动 寻找 木 箱 并 将 其 
推 到 指定 位 置 。 但 是 ,控制 它 的 计算 机 体积 庞大 ， 
运算 速度 慢 ,导致 Shakey 往往 需要 数 小 时 来 分 析 
环境 并 规划 运动 路 径 。Shakey 被 认为 是 世界 上 第 

图 1.12 Shakey 机 器 人 0 一 台 智 能 机 器 人 ,由 此 拉 开 了 第 三 代 机 器 人 研发 
的 序幕 。 

1969 年 ,通用 汽车 公司 在 Lords-town 装配 厂 安装 了 首 台 Unimation 点 焊 机 器 人 ,机 器 

人 的 使 用 大 大 提高 了 生产 效率 ,90% 以 上 的 车 身 焊接 作业 可 通过 机 器 人 来 自动 完成 。 


第 1 章 结论 |p 7 


1969 年 ,挪威 Trallfa 公司 (后 被 ABB 公司 收购 ) 推 出 了 第 一 个 商业 化 应 用 的 喷漆 机 器 
人 ,如 图 1.13 所 示 。 在 1967 年 挪威 劳动 力 短缺 期 间 , 该 公司 就 曾 使 用 机 器 人 来 喷涂 独 轮 手 
推 车 (Wheelbarrows) ,第 一 款 商 用 喷漆 机 器 人 就 由 此 发 展 而 来 。 

1969 年 ,Unimation 公司 与 日 本 川崎 重工 (Kawasaki Heavy Industries) 签 订 许可 协议 ， 
在 日 本 生产 Unimate 机 器 人 ,同年 川崎 重工 成 功 开 发 出 Kawasaki-Unimate 2000 机 器 人 ,这 
是 日 本 生产 的 第 一 台 工 业 机 需 人 ,如 图 1.14 所 示 。 


图 1.13 Trallía 喷漆 机 器 人 图 1.14 Kawasaki-Unimate 2000 机 器 人 中 


1969 年 ,斯 坦 福 大 学 机 械 工 程 系 学 生 Victor Scheinman 设计 出 了 Stanford Arm, 如 
图 1.15 所 示 ,这 是 机 器 人 发 展 历 史上 的 第 一 个 全 电 驱 动 的 6 轴 机 器 人 (5 轴 转 动 ,1 轴 移 
动 ) 6 轴 机 器 人 的 出 现 , 使 得 跟踪 空间 中 的 任意 路 径 成 为 可 能 ,推动 了 机 器 人 向 更 加 复杂 
领域 的 应 用 ,如 装配 、 弧 焊 等 。 该 机 器 人 的 出 现 是 机 器 人 ,尤其 是 工业 机 器 人 发 展 历 程 上 的 
里 程 碑 性 事件 。 


图 1.15 Stanford Arm! Victor Scheinman ` 


1969 #E, H K EL 88 HI X 2 Jill eE (Ichiro Kato) 教授 成 功 研 发 出 第 一 台 以 双 足 行走 
的 机 器 人 WAP-1, 如 图 1.16 所 示 。WAP-1 采用 橡胶 制 成 的 人 工 肌 肉 作为 驱动 器 ,实现 
了 双 足 在 平面 上 的 运动 。1973 年 ,加 有 藤 一 郎 又 开发 出 世界 上 第 一 个 全 尺寸 的 仿 人 机 器 人 
WABOT-1, 如 图 1.17 所 示 ,该 机 器 人 有 视觉 和 语音 对 话 系 统 ,能 以 日 语 与 人 对 话 , 能 搬 
运 物品 ,其 智力 与 一 岁 半 儿 童 相当 。 加 芯 一 郎 作 为 仿 人 机 器 人 的 先驱 ,长 期 致力 于 仿 人 
机 器 人 研究 ,为 仿 人 机 器 人 的 研究 做 出 大 量 开创 性 的 工作 ,被 誉 为 “ 仿 人 机 器 人 之 父 ”。 
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运动 学 、 动力 学 及 运 云 动 规划 


图 1.16 WAPU“ LRA 155 Jl g£ — B 22 Z 图 1.17 WABOT-1 #L28 A0 


1970 年 11 月 17 日 ,苏联 的 “月 球 17 号 ”( 俄 文 : JIyaa-17) 探 测 器 把 世界 上 第 一 台 无 人 
月 球 表面 巡视 机 大 人 “月 球 车 1 号 ”(Lunkhood 1) 送 到 了 月 球 ,如 图 1. 18 所 示 ; 第 一 次 
实现 了 在 地 球 上 对 另 一 个 星球 上 机 器 人 的 远程 和 遥控。 “月 球 车 1 号 ?主要 由 仪器 舱 和 自动 行 
走 底盘 组 成 , 重 756kg, 长 2.2m, 宽 1.6m, 高 1.35m, 由 太阳 和 角 a WN a pea 全 供电 
由 同位 素 热 源 保持 系统 温度 ,车 上 装 有 电视 摄像 机 和 多 种 环境 科学 测量 仪器 ， 可 把 拍摄 的 月 
面 照 片 和 测量 结果 发 回 地 球 。“ 月 球 车 1 号 ”由 两 列 独立 驱动 的 车 轮 ( 每 列 4 个 ) 实 现在 月 面 
的 运动 ,能 转弯 .倒退 和 爬 上 30° 的 斜坡 ,轮子 直径 0.51m。“ 月 球 车 1 号 ”在 月 球 表 面 工作 到 
1971 年 10 月 4 日 ,总 行程 约 10. 54km, 

1970 年 ,在 美国 召开 了 第 一 届 国 际 工 业 机 器 人 学 术 会 议 。 一 年 以 后 ,机 器 人 的 研究 得 
到 迅速 广泛 的 普及 。 

1973 4 s S R AE “se 公司 辛辛那提 。 米 拉克 隆 公 司 (CCincinnati Milacron Inc. ) 
的 理 查 德 ， 聚 恩 制造 了 一 iei iia raii T3, 如 图 1.19 所 示 , 它 采用 液 
压 驱 动 , f "É 的 有 效 4 45kg。 


图 1.18 月 球 车 1 号 ne 图 1.19 T3 机 器 人 中 


1973 年 ,德国 库 卡 公司 (KUKA) 研 发 出 第 一 台 工 业 机 器 人 ,命名 为 Famulus, 如 图 1. 20 
所 示 , 这 是 世界 上 第 一 台电 机 驱动 的 6 轴 工 业 机 器 人 。1973 年 全 世界 运行 的 工业 机 器 人 数 
量 达 到 3000 £ 
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1974 年 ,瑞典 通用 电机 公司 ASEA(ABB 公司 的 前 身 ) 开 发 出 世界 上 第 一 人 台 全 电 驱 
动 `. 由 微 处 理 器 控制 的 工业 机 器 人 IRB 6, 如 图 1.21 所 示 , 主 要 应 用 于 工件 取 放 和 物料 搬 
和 运 。 该 机 器 人 负载 6kg, 使 用 英特尔 8 位 微 处 理 器 控制 ,该 微 处 理 器 的 内 存 容 量 仅 
16KB。 


图 1.20 Famulus 机 器 人 图 1.21 IRB 6 机 器 他 


1975 年 ,意大利 的 Olivetti 公司 开发 出 直角 坐标 机 器 人 SIGMA ,如 图 1. 22 所 示 , 它 是 
一 个 应 用 于 组 装 领 域 的 工业 机 器 人 ,在 意大利 一 家 组 装 厂 安装 运行 。 

1978 年 ,日 本 山梨 大 学 (University of Yamanashi) 的 牧野 洋 (Hiroshi Makino) 教 授 发 
BH íf SCARA (Selective Compliance Assembly Robot Arm) 机 器 人 , 意 为 选择 柔性 装配 机 械 
臂 , 如 图 1.23 所 示 。1981 年 ,Sankyo Seiki 公司 和 Nitto Seiko 公司 分 别 开 发 出 了 商业 化 的 
SCARA 机 器 人 产品 。SCARA 机 器 人 一 般 有 4 个 自由 度 (3 个 转动 ,1 个 移动 ) ,特别 适合 于 
轻型 物品 的 快速 转移 和 装配 。 


图 1.22 SIGMA 机 器 人 六 图 1.23 SCARA 机 器 人 区 


1978 年 ,美国 Unimation 公司 推出 由 Victor Scheinman 主持 设计 的 通用 工业 机 器 人 
PUMA (Programmable Universal Machine for Assembly) ,如 图 1. 24 所 示 , 并 应 用 于 通用 
汽车 装配 线 , 这 标志 着 工业 机 器 人 技术 已 经 完全 成 熟 。 
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ee 


图 1.24 PUMA 机 器 人 [> 


1.2.3 现代 机 器 人 的 发 展 


20 世纪 80 年 代 ,工业 机 器 人 独 领 风骚 ,在 制造 业 尤 其 是 汽车 制造 行业 得 到 了 大 规模 的 
推广 和 应 用 。20 世纪 90 年 代 以 后 ,各 种 新 型 的 服务 机 器 人 开始 逐渐 出 现 , 并 逐步 发 展 到 今 
天 机 器 人 行业 百花 齐 放 的 局 面 。 下 面 对 这 期 间 出 现 
的 典型 机 器 人 做 简单 介绍 。 

1981 年 ,美国 宇航 局 将 世界 上 第 一 款 大 型 空间 
遥控 机 械 臂 CanadArm 1( 加 拿 大 辟 1 aT 
伦比 亚 号 航天 飞机 (STS-2) 上 进行 了 在 轨 测 试 。 
机 械 臂 由 加 拿 大 国家 人 研究 委员 会 (Canadian 
National Research Council) 历 时 六 年 组 织 研 制 , 安 
装 在 航天 飞机 货舱 的 一 侧 , 主 要 用 在 航天 飞机 上 进 
行 展开 、 操 纵 和 捕获 物品 等 作业 。CanadArm 1 重 
410kg, 长 15. 2m, 直 径 38cm, 结 构 上 与 人 的 手臂 类 
似 , 分 成 3 节 , 共 有 6 个 自由 度 , 能 搬运 332. 5kg 的 图 1.25 CanadArm 1 (STS-135)"^ 
有 效 载荷 ,20 世纪 00 年 代 中 期 有 效 载荷 增加 至 3293kg, 如 图 1. 25 所 示 。 机 械 辟 上装 有 电 
视 摄像 机 和 照明 设备 ,座舱 内 的 航天 员 通 过 摄像 机 传 过 来 的 前 方 图 像 , 操 纵 机 械 辟 完成 抓举 
或 释放 任务 。 加 拿 大 总 共 为 NASA( 美 国 国家 宇航 局 ) 研 制 了 5 个 这 种 机 械 臂 (201,202， 
301,302,303), 其 中 302 号 臂 在 “挑战 者 ”号 航天 飞机 爆炸 事故 中 被 毁 。2011 年 7 月 
CanadArm 1 最 后 一 次 随 航 天 飞机 执行 任务 (STS-135), 这 也 是 它 的 第 90 次 任务 。 在 航天 

飞机 投入 使 用 的 20 多 年 中 ,该 机 械 辟 完成 了 上 百 次 
的 释放 、 回 收 卫 星 以 及 协助 航天 员 在 太空 维修 航天 
器 的 任务 。 

1982 年 , Salisbury 设计 了 具有 代表 性 意义 的 
Stanford/JPL 多 指 灵 巧 手 , 如 图 1.26 所 示 。 
Stanford/JPL mira 3 根 手指 ,共有 9 个 自由 度 ， 
采用 腿 驱 动 方式 ,每 根 手 指使 用 n 十 1 Ë (n 个 关节 ) 
图 1.26 Stanford/JPL 多 指 灵巧 手 c 传动 设计 ,使 用 和 4 条 腿 驱 动手 指 的 3 个 关节 。 
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Stanford/JPL 手 集成 了 位 置 传感器 和 基于 应 变 片 的 指 尖 力 /触觉 传感器 ,将 位 置 和 力 信 息 
反馈 引入 了 灵巧 手 控制 策略 ,并 进行 了 力 控制 和 刚度 控制 的 抓 取 操作 实验 以 及 力 和 被 抓 取 
物体 形状 的 感知 实验 。Stanford/JPL 手 的 驱动 控制 器 及 主 控制 器 均 放 置 于 灵巧 手 外 部 , 采 
用 集中 控制 的 方式 进行 抓 取 操作 控制 。 

1984 年 ,日 本 早稻 田 大 学 研制 出 世界 首 台 弹 电子 琴 的 人 形 机 器 人 WABOT-2, 如 图 1. 27 
所 示 。 该 机 器 人 高 189cm, E 82kg, 配 备 两 只 五 指 的 仿 人 手 , 能 够 与 人 交谈 ,能 用 视觉 相机 
读 乐 谱 ,能 在 电子 其 上 用 手指 弹 奏 中 等 难度 的 乐曲 。1985 年 ,该 机 器 人 在 日 本 “ 科 博 会 - 筑 
波 85? 展 上 演奏 了 电子 琴曲 目 , 在 世界 范围 内 引起 很 大 关注 。 

1985 年 ,犹他 大 学 和 麻 省 理工 学 院 研制 了 Utah/MIT 灵巧 手 。 如 图 1. 28 所 示 , Utah/ 
MIT 灵巧 手 采 用 了 模块 化 设计 的 理念 ,共有 4 根 手 指 、16 个 自由 度 。 该 灵巧 手 采 用 腿 加 滑 
轮 传动 ,采用 2n 条 腿 传 动 设计 模式 ,总 共 使 用 了 32 条 腿 , 采 用 气 氏 驱动 ,具有 基于 霍 尔 效应 
的 位 置 传感器 和 基于 应 变 片 的 腿 张 力 传 感 涡 。Utah/MIT 灵巧 手 采用 分 层 控制 的 方式 , 抓 
取 和 运动 规划 层 采用 Sun 工作 站 进行 运算 ,协调 运动 控制 层 由 集成 微 处 理 器 完成 ,由 Jife 驱 
动 控制 模块 实现 驱动 器 控制 。 


图 1.27 WABOT-2 机 器 人 图 1.28 Utah/MIT RIJP” 


1993 年 ,德国 宇航 中 心 在 哥伦比亚 航天 飞机 上 (Spacelab D2) 开 展 了 一 项 名 为 ROTEX 
LE 1. 29) 的 舱 内 机 器 人 实验 。 利 用 一 台 安 装 在 空间 实验 室 支 架 上 的 带 有 多 传感器 手 爪 


图 1.29 ROTEX" 
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的 6 自由 度 机 械 臂 ,进行 了 装配 机 械 构架 、 插 拔 在 轨 可 更 换 单元 、 托 取 尖 溯 物体 等 实验 ,并 测 
试 了 预 编 程 自动 控制 .在 轨 遥 操作 、 地 面 远 程 怕 操作 等 不 同 的 控制 方式 ,为 空间 遥 操 作 机 器 
人 的 研究 和 应 用 提供 了 重要 参考 。 

1996 年 ,本 田 公司 经 过 10 年 研发 终于 研制 出 世界 上 第 一 
台 真 正 意义 上 的 人 形 双 足 步行 机 器 人 P2, 如 图 1. 30 所 示 Z 
机 器 人 总 重 210kg, 高 182cm。 它 能 够 用 两 条 腿 平 稳 上 下 楼 
梯 ,用 扳手 拧 螺 丝 ,被 称 为 “第 一 台 双 足 自律 步行 机 器 人 ”。 该 
成 果 在 各 国 研究 人 员 中 引起 极 大 和 麦 se, Pte Y ta Bl pi A 
JÉ HLAS À (Humanoid Robot) 的 研究 热潮 。1997 年 ,时 任国 务 
院 总 理 李鹏 一 行 在 丰田 公司 总 部 参观 了 该 机 器 人 。 

1997 年 7 月 4 日 ,美国 国家 宇航 局 于 1996 年 12 月 4 日 发 
射 的 探测 机 器 人 索 杰 纳 (Sojourner) 成 功 登 陆 火星 ,如 图 1. 31 所 
示 , 这 是 世界 上 第 一 台 自 主 式 星球 探测 机 器 人 , 它 能 利用 激光 
传感器 和 摄像 机 识别 环境 障碍 ， | 该 机 器 
AEH 11. 5kg, 长 0.63m, 宽 0. 48m, 最 大 速度 0. ea 
共有 6 个 独立 悬挂 驱动 的 车 轮 ,每 个 车 轮 直 径 13cm ,前 后 轮 均 能 独立 转向 ， S 
形 上 运动 。 

1997 4Æ 11 月 28 日 ,日 本 宇宙 开发 事业 团 (NASDA) 发 射 了 国际 上 第 一 个 自由 飞行 空 
间 机 器 人 系统 一 一 工程 试验 卫星 7 号 (ETS VII) ,如 图 1. 32 所 示 , 该 系统 由 追踪 卫星 和 目 
标 卫星 mae eah 追踪 卫星 上 装 有 一 条 长 约 2m、6 个 自由 度 的 机 械 臂 ,未 端 安装 有 长 约 
15cm 的 三 J 手 。 首 次 试验 了 无 人 干预 情况 下 ,目标 星 远离 20cm 后 经 过 位 姿 测量 再 用 
nn 由 度 .多 传感器 机 械 手 在 空间 在 轨 服 务 的 可 行 性 。 
所 有 空间 试验 在 1999 年 底 完成 。 


图 1.30 P2 机 器 人 5 


图 1.31 索 杰 纳 机 器 人 5 图 1.32 ETS VII? 


1998 年 ,德国 宇航 中 心 基于 研制 成 功 的 新 型 驱动 器 ,设计 了 第 一 个 真正 的 完全 内 置 
式 多 指 灵 巧 手 DLR LI, 它 有 4 个 手指 , 共 12 个 自由 度 , 如 图 1.33 所 示 。DLR I 在 手指 末 
端 关节 采用 了 妥 驱 动 , 所 有 驱动 及 传动 装置 .控制 系统 、 传 感 器 及 通信 系统 均 集 成 在 灵巧 


A 第 1 章 绪论 |P 13 
手 的 内 部 ,使 灵巧 手 独 立 于 机 械 辟 成 为 一 个 模块 化 的 局 部 自主 
1999 年 ,美国 国家 宇航 局 和 通用 电气 公司 设计 开发 了 世界 
上 第 一 个 面向 太空 任务 的 仿 人 机 器 人 Robonaut 1 (R1), 包 括 
RIA 和 RIB 两 个 版 本 ,如 图 1. 34 (a) 所 示 。R1l 共有 47 个 自由 
度 , 其 中 , 颈 部 有 2 个 自由 度 , 两 条 臂 各 有 7 个 自由 度 , 每 条 臂 末 
端 携带 的 五 指 灵巧 手 有 12 个 自由 度 , 还 有 一 条 7 自由 度 的 腿 ,用 
于 固定 在 空间 站 上 。 设 计 R1 的 目的 是 可 以 替代 宇航 员 完 成 各 种 
枯燥 ,重复 甚至 危险 的 任务 。 此 外 ,可 以 根据 不 同 工 作 环境 更 换 
RI 下 半身 结构 ,如 果 在 空间 站 工作 ,Rl 的 下 半身 为 零 重力 腿 ; 
如 果 在 地 面 工 作 ,Rl 的 下 半身 则 可 更 换 为 轮 式 移动 平台 。2011 
年 ,美国 国家 宇航 局 和 通用 电气 公司 通过 对 R1 的 改进 ,成功 研 制 出 新 一 代 的 太空 仿 人 机 器 
人 Robonaut 2(R2) ,并 成 为 第 一 个 进入 国际 空间 站 的 仿 人 机 器 人 。R2 共有 42 个 自由 度 ， 
其 中 颈 部 有 3 个 自由 度 ,两 条 臂 各 有 7 个 自由 度 ,每 条 臂 末端 携带 的 五 指 灵巧 手 有 12 个 自 
由 度 ,还 有 1 个 腰 转 自由 度 。R2 的 动作 速度 是 Rl 的 4 倍 ,整体 设计 更 加 紧凑 ,更 具 灵 活性 ， 
是 具备 更 广 的 感知 范围 。R2 最 大 的 特点 就 是 拥有 类 似 人 的 灵巧 手 , 相 比 于 R1,R2 拇指 的 自 
由 度数 增加 到 4 个 ,手指 灵巧 性 提高 了 40% ,使 其 能 够 抓 握 更 多 的 工具 。 此 外 ,R2 手指 的 每 个 
关节 都 集成 了 6 维 力 / 力 和 矩 传感器 以 及 腿 张 力 传感器 ,显著 提高 了 灵巧 手 的 力 控 和 传 感 性 能 ，。 


图 1.33 DLR I 灵巧 手 * 
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图 1.34 Robonaut 机 器 人 


1999 年 :日 本 索尼 公司 推出 可 与 用 户 互 动 的 宠物 狗 玩具 机 器 人 爱 宝 (AIBO, Artificial 
Intelligence Robot) ,型 号 ERS-110, 售 价 超过 2000 美元 ,日 本 首发 的 3000 £ fE 20 分 钟 内 
销售 一 空 。 爱 宝 机 器 人 有 18 个 自由 度 ,其 中 嘴 部 1 个 自由 度 , 头 部 3 个 自由 度 , 每 条 腿 3 个 
自由 度 ,尾巴 2 个 自由 度 。 头 部 和 四 个 爪 端 各 有 一 个 触觉 传感器 , 头 部 还 有 一 个 CCD 相机 。 
爱 宝 机 器 人 在 摔 倒 后 能 自动 复位 ,还 能 通过 LED 灯光 表达 高 兴 ( 绿 色 ) 和 生气 (红色 ) 等 情 
绪 。 需 要 说 明 的 是 ,AIBO 机 器 人 共有 5 种 不 同 的 外 形 , 如 图 1. 35 所 示 。AIBO 机 器 人 总 销 
量 超过 15 万 台 ,2015 年 它 与 日 立 8 位 计算 机 一 起 人 选 了 日 本 的 “未 来 技术 遗产 ”。 
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图 1.35 AIBO 机 器 人 和 


2000 年 ,美国 食品 药品 监督 管理 局 (FDA) 批 准 了 Intuitive Surgical 公司 开发 的 达 。 芬 
奇 外 科 手 术 机 器 人 系统 (da Vinci rp System) 在 泌尿 外 科 等 手术 中 的 应 用 ,由 此 拉 开 
了 医疗 机 器 人 商业 化 应 用 的 序幕 。 达 ， 芬 奇 机 器 人 由 三 部 分 组 成 : 外 科 医 生 控 制 台 、 床 旁 
机 械 辟 系统、 成 像 系统 ,如 图 1. 36 ew 手术 过 程 中 主刀 医生 坐 在 控制 台 旁 ,使 用 双手 操作 
两 个 主 臂 控制 床 旁 机 械 臂 系统 在 患者 体内 的 手术 。 床 旁 机 械 臂 系统 一 般 由 2 一 3 只 器 械 辟 

1 只 摄像 臂 组 成 ,器 械 臂 有 7 个 自由 度 , 可 以 模拟 人 手 完 成 各 种 操作 。 截 至 2014 年 6 月 
30 日 ,全 世界 约 有 3102 台 达 。 芬 奇 手术 机 器 人 。 

2000 年 ,本 田 公 司 推出 了 新 一 代 人 形 机 器 人 阿 西 莫 (ASIMO,Advanced Step Innovative 
Mobility ,高 级 步行 创新 移动 能 力 ) ,高 120cm, 重 52kg, 行 走 速度 1. 6km/h, 如 图 1. 37 所 示 。 
与 本 田 公司 早先 研究 的 人 形 机 器 人 相 比 , 阿 西 莫 具 有 体型 小 .重量 轻 .动作 轻柔 灵活 等 特点 ， 
更 适合 在 家 居 环 境 中 应 用 。 现 在 阿 西 莫 机 器 人 已 发 展 到 第 三 代 (2011 年 推出 ), 它 不 但 能 跑 、 
走 、 上 下 阶梯 ,还 会 踢 足 球 MIE AR ,动作 十 分 灵巧 ,代表 着 当前 人 形 机 器 人 的 顶尖 水 平 。 


图 1. 36 达 芬 奇 外 科 手 术 机 器 人 系统 5 图 1.37 ASIMO 机 器 人 cl 


2001 年 4 月 19 日 ,CanadArm 2( 加 拿 大 臂 2 号 ) 机 械 臂 由 “奋进 号 ?航天 飞机 (STS-100) 
带 上 太空 ,安装 在 国际 空间 站 上 , 主要 用 于 空间 站 的 装配 和 维护 。CanadArm 2 是 国际 空 
间 站 的 “移动 服务 系统 ”(Mobile Servicing System, MSS) 的 组 成 部 分 之 一 , 即 空间 站 遥控 机 
械 臂 系统 (Space Station Remote Manipulator System,SSRMS) 。CanadArm 2 有 7 个 关节 ， 
总 长 17. 6m, 直径 35cm, 自重 1800kg, 最 大 负荷 116t, 如 图 1. 38 所 示 。CanadArm 2 比 
CanadArm 1 更 长 ,负载 更 大 , 它 可 以 帮助 航天 飞机 停靠 在 国际 空间 站 ,搬运 设备 和 物资 , 帮 
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助 宇 航 员 在 太空 工作 。 加 拿 大 2 号 臂 与 加 拿 大 1 号 臂 最 大 的 区 别 在 于 ,2 号 臂 两 头 都 有 抓 固 装 
EE ,任何 一 端 都 可 以 沿 柏 架 上 的 导轨 移动 到 新 位 置 后 再 固定 ,而 1 号 臂 只 有 一 端 有 抓 固 装置 。 

2003 年 ,索尼 公司 推出 QRIO (Quest for cuRIOsity) 人 形 机 器 人 ,高 约 0. 6m, 重 约 
7. 3kg, 步 行 速 度 0. 8km/h, 如 图 1.39 所 示 。QRIO 可 以 跑步 .跳舞 、 唱 歌 . 踢 足球 ,并 能 与 人 
互动 ,是 世界 上 第 一 个 可 以 跑 的 双 足 人 形 机 器 人 。 如 果 QRIO 跌倒 了 , 它 会 自己 站 起 来 。 
因为 索尼 公司 自身 业务 调整 ,2006 年 1 月 索尼 公司 停止 了 AIBO 和 QRIO 的 研发 和 销售 ， 
这 也 是 机 器 人 发 展 历史 中 的 一 大 居 事 。 


图 1.38 CanadArm 2: 图 1.39 QRIO 机 器 人 '” 


2002 年 ,美国 iRobot 公司 推出 了 吸尘器 机 器 人 Roomba, 如 图 1. 40 所 示 。 它 能 避 开 障 
碍 ,自动 设计 行进 路 线 , 还 能 在 电量 不 足 时 ,自动 驶 向 充电 插座 。2015 年 推出 的 Roomba 
980 采用 了 VSLAM(Vision Simultaneous Localization and Mapping) 技 术 , 在 清洁 房间 的 
同时 能 绘制 房间 地 图 ,从 而 知道 哪些 地 方 需要 清洁 、 哪 些 地 方 清洁 不 到 ,这 样 在 下 次 清理 时 能 
够 加 快速 度 , 节 省 时 间 。Roomba 共有 6,8,9 三 个 系列 ,是 目前 世界 上 销量 最 大 的 家 用 机 器 人 。 

2005 年 ,美国 波士顿 动力 公司 (Boston Dynamics) 推 出 了 一 款 动 态 稳定 性 超 强 的 四 足 
机 器 人 “大 狗 ”(BigDog) ,如 图 1. 41 所 示 。 大 狗 机 器 人 高 约 1m, 重 约 109kg, 负 载 45kg, 采 
用 汽油 机 作为 动力 ,采用 液压 入 驱动 16 个 关 市 。 该 机 融 人 能 在 雪 地 、 冰 面 、 碎 石 地 、 林 地 等 
野外 环境 中 稳定 行走 ,即使 受到 外 部 撞击 也 能 不 摔 倒 。 该 机 器 人 实验 视频 一 经 推出 即 引 起 
很 大 龙 动 ,引发 了 新 一 轮 四 足 机 器 人 的 研究 热潮 。 大 狗 机 器 人 被 洽 为 世界 上 第 一 台 能 在 真 
实 世 界 环境 而 非 实 验 室 中 稳定 运动 的 四 足 机 器 人 。 


图 1.40 Roomba 图 1.41 BigDog"' 
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2006 年 6 月 ,微软 公司 推出 Robotics Developer Studio ,机 器 人 模块 化 .平台 统一 化 的 
趋势 越 来 越 明 显 。 比 尔 ， 盖 茨 曾 预 言 , 家 用 机 器 人 将 来 会 像 个 人 计算 机 那样 进入 每 个 家 庭 。 

2009 年 ,丹麦 的 优 做 (Universal Robots) 公 司 推出 了 世界 上 第 一 款 人 机 协作 机 器 人 
UR5 ,如 图 1. 42 所 示 。 该 机 器 人 是 一 种 6 关节 的 轻 量 型 机 器 人 , 重 !18kg, 负 载 5kg, 工 作 半 
径 85cm, 重 复 定位 精度 士 0. 1mm。 该 机 器 人 的 最 大 特点 是 能 够 与 人 近 距 离 一 起 工作 ,一旦 
与 人 接触 并 产生 150N 以 上 的 力 时 ,该 机 器 人 能 自动 停止 ,从 而 避免 对 人 产生 伤害 。UR5 机 
器 人 推出 后 ,迅速 在 世界 范围 内 掀起 了 人 机 协作 机 器 人 的 研究 和 开发 热潮 。 目 前 , 优 做 已 成 
为 世界 排名 第 一 的 人 机 协作 机 器 人 公司 。 

2015 年 ,ABB 机 器 从 公司 在 德国 汉诺威 工业 博览 会 上 推出 全 球 首 款 人 机 协作 双 臂 机 
器 人 YuMi, 如 图 1. 43 所 示 ,该 名 字 来 源 于 You & Me, YuMi 机 器 人 采用 铝 镁 合金 骨架 ， 
外 包 覆 软 性 材料 和 塑料 外 壳 , 能 够 很 好 地 吸收 外 部 的 冲击 。 每 个 臂 有 7 个 自由 度 ,并 带 一 个 
1 自由 度 的 手 爪 ,负载 500g。 该 机 器 人 具有 力 控制 功能 ,在 碰 到 人 或 其 他 物体 时 能 够 自动 停 
下 来 ,不 需要 像 其 他 工业 机 器 人 那样 与 人 隔离 。 


图 1.42 UR5®? 图 1.43 YuMics3] 


1.2.4 国内 机 器 人 的 发 展 


我 国 的 机 融 人 研究 起 步 于 20 世纪 70 年 代 初 期 ,经 过 "七 五 ?重点 攻关 、 八 五 "应 用 工程 
开发 及 “863 计划 ?实施 ,逐渐 从 最 初 缓慢 的 自主 研发 转变 成 国家 重视 的 有 计划 研究 .开发 和 
推广 应 用 的 阶段 。 我 国 机 器 人 的 发 展 过 程 大致 可 分 为 三 个 阶段 : 20 世纪 70 年 代 的 萌芽 期 、 
80 年 代 的 发 展期 和 90 年 代 至 今 的 繁荣 期 。 

20 世纪 70 年 代 初 ,我 国 开始 关注 机 器 人 的 发 展 , 并 开始 尝试 机 器 人 的 研发 。1977 年 ， 
全 国 机 械 手 技术 交流 大 会 在 浙江 嘉兴 召开 ,这 是 我 国 历史 上 第 一 个 以 机 融 人 为 主题 的 大 型 
学 术 会 议 , 开 启 了 我 国 机 器 人 学 术 交 流 的 新 纪元 。 参 会 人 员 表 达 了 我 国 机械 工 业界 对 发 展 
机 器 人 技术 的 殷切 渴望 ,为 促进 日 后 中 外 学 术 交 流 活动 的 广泛 开展 、 机 器 人 研究 机 构 的 建立 
及 推动 我 国 机 器 人 战略 的 实施 打下 了 坚实 基础 。 

1978 年 ,应 中 日 友好 协会 邀请 ,日 本 早稻 田 大 学 的 著名 机 器 人 专家 加 蕨 一郎 教授 来 北 
京 访问 交流 ,从 此 打开 了 我 国 机 器 人 对 外 交流 的 窗口 。 此 后 ,美国 .日 本 等 国家 的 教授 陆续 
来 华 访 问 交 流 ,我 国 机 器 人 领域 的 专家 也 开始 走出 国门 。 

为 加 快 机 器 人 研究 步伐 , 自 20 世纪 70 年 代 末 至 1985 年 ,国内 先后 在 航空 部 .机械 部 、 
中 科 院 沈阳 自动 化 所 及 多 所 高 校 成 立 机 器 人 科研 机 构 , 开 展 机 右 人 的 研发 工作 ,并 协助 国家 
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主管 部 门 开展 有 关机 器 人 发 展 战略 的 规划 工作 。 “七 五 计划 ”期间 ,国家 将 "工业 机 器 人 开发 
研究 ”作为 重大 科技 攻关 项 目 , 重 点 对 点 焊 、 弧 焊 、 喷 漆 、 搬 运 等 型 号 的 工业 机 器 人 及 其 零 部 
件 进行 攻关 ,形成 了 中 国 工业 机 器 人 的 第 一 次 研发 高 潮 。 
1985 年 ,哈尔滨 工业 大 学 蔡 稚 皋 教授 团队 率先 研制 出 我 国 第 一 台 弧 焊 机 器 人 一 一 华宇 
[型 CHY- 工 ) 弧 焊 机 器 人 ( 见 图 1. 44) ,两 年 之 后 又 研制 出 国内 第 一 台 点 焊 机 器 人 HRGD-1 
型 点 焊 机 器 人 ( 见 图 1.45). 


图 1.44 华宇 I 型 弧 焊 机 器 人 一 图 1.45 HRGD-1 型 点 焊 机 器 人 [9 


1985 年 ,中科院 沈阳 自动 化 研究 所 阔 新 松 研究 员 主 持 研制 了 “ 海 人 一 号 ”100m 水 下 机 
器 人 ,并 先后 于 1985 年 及 1986 年 获得 首 航 及 深 潜 试 验 的 成 功 ,在 技术 上 达到 了 20 世纪 80 
年 代 国 际 同类 产品 水 平 。 

1986 年 ,国家 高 技术 研究 发 展 计划 (863 计划 ) 开 始 实施 ,确定 了 特种 机 器 人 与 工业 机 咒 
人 并 重 的 发 展 方针 ,项 目 实施 期 间 共 研制 开发 出 7 种 工业 机 器 人 和 102 种 特种 机 器 人 。 

从 1987 年 开始 ,北京 航空 航天 大 学 的 张 启 先 教 授 带 领 团队 持续 开展 了 机 器 人 仿生 灵巧 
手 的 研究 ,并 于 20 世纪 90 年 代 初 研制 出 了 BH-1 三 指 9 自由 度 灵巧 手 , 填 补 了 当时 的 国内 
空白 ,并 于 1991 年 4 月 参加 了 国家 科 委 主办 的 863 计划 五 周年 成 果 展 ,后 来 又 陆续 研制 出 
BH-2.BH-3 灵巧 手 ( 图 1.46), 1993 年 , 张 启 先 教授 完成 了 国内 首 个 7 自由 度 完 余 机 器 人 
样机 的 研制 ,如 图 1. 47 所 示 ， 此 项 成 果 不 仅 在 我 国 处 于 领先 地 位 ， 而 且 在 某 些 方面 达到 了 
20 世纪 80 年 代 末 国际 先进 水 平 。 


图 1.46 BH-3 灵巧 手 图 1.47 张 启 先 院士 和 7 自由 度 机 器 人 
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1990 年 ,北京 机 械 工业 自动 化 研究 所 成 功 研 制 出 我 国 第 一 台 喷 漆 机 器 人 PJ-1, 其 主要 
性 能 达到 20 世纪 80 年 代 中 期 国外 同类 产品 水 平 。 - 

1994 年 ,中 科 院 沈阳 自动 化 研究 所 研制 成 功 “ 探 索 者 ”号 水 下 机 器 人 ( 见 图 1. 48) ,其 工 
作 深 度 达 到 了 1000m ,并 且 甩 掉 了 与 母 船 间 联 系 的 电缆 ,实现 了 我 国 水 下 机 器 人 从 有 缆 向 无 
WA KER. 

除 此 之 外 ,我 国 还 先后 研制 出 了 具有 自主 知识 产权 的 点 焊 、 弧 焊 、 装 配 、 喷 漆 、 切 割 、 搬 
运 、 包装 码 埃 等 用 途 的 工业 机 器 人 ,并 实施 了 一 批 机 器 人 应 用 工程 ,形成 了 一 批 机 器 人 产业 
化 基地 ,为 我 国 机 器 人 产业 的 腾飞 葛 定 了 基础 。“863 计划 ”的 成 功 实施 ,使 我 国 机 器 人 技术 
的 研究 .开发 .应 用 和 产业 化 从 最 开始 的 无 序 分 散 、 低 水 平 重复 状态 推进 到 初 具 行 业 性 的 新 
阶段 。 从 此 以 后 ,中 国 机 器 人 产业 开始 逐渐 走向 规范 化 、 规 模 化 。 

随 着 中 国 经 济 的 快速 发 展 、 科 技 水 平 的 不 断 提高 及 制造 工艺 的 日 益 成 熟 , 进 入 21 世纪 ， 
中 国 机 器 人 产业 迎 来 了 第 二 次 发 展 的 高 潮 期 ,国内 机 器 人 公司 纷纷 成 立 , 开 始 研发 各 类 机 器 
人 产品 。 与 此 同时 ,企业 与 高 校 . 科 研 机 构 之 间 广 泛 建立 合作 关系 ,掀起 了 一 股 新 的 机 器 人 

2013 年 12 H 15 日 ,我 国 研制 的 玉兔 号 月 球 车 成 功 登陆 月 球 表面 ,成 为 继 美 国 、 苏 联 
之 后 第 三 个 登陆 月 球 的 国家 。 玉 人 免 号 月 球 车 长 1. 5m,; 宽 lm, 高 1. 1m, 重 约 140kg, 具 备 
Jë 20 斜坡 和 越 20cm 障碍 的 能 力 ,配备 了 全 景 相机 、 红 外 成 像 光谱 仪 、 测 月 雷达 等 仪器 ,如 
图 1.49 所 示 。 


图 1.48 “探索 者 ”号 水 下 机 器 人 图 1.49 ESARET 


综 上 所 述 , 从 应 用 领域 来 看 ,我 国 机 右 人 的 研究 已 经 由 早先 的 工业 应 用 扩展 到 更 多 的 非 
工业 应 用 领域 ,各 种 类 型 的 服务 机 器 人 层出不穷 ; 从 发 展 方向 上 来 看 ,从 最 早 只 能 执行 简单 
程序 .重复 简单 动作 的 工业 机 器 人 向 有 具有 感知 能 力 的 智能 型 机 器 人 发 展 。 


1.3 机 器 人 的 专业 术语 


机 器 人 中 有 一 些 专业 术语 ,了 解 和 掌握 这 些 有 助 于 更 好 地 学 习 机 器 人 的 相关 知识 ,下 面 
介绍 一 些 重 要 的 机 器 人 专业 术语 。 

1. 坐标 系 (EFrame/Coordinate Frame) 

在 机 器 人 学 中 ,坐标 系 具有 非常 重要 的 作用 ,是 建立 机 器 大 数学 模型 的 基础 。 在 机 器 人 
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学 中 广泛 采用 的 是 空间 笛 卡 儿 直 角 坐 标 系 , 即 满足 “右手 系 ” 的 三 维 正 交 坐 标 系 , 通 常 被 简称 
为 第 卡 儿 坐标 系 , 如 图 1. 50 所 示 。 

两 条 相交 于 原点 且 度 量 单位 相等 的 数 轴 所 
构成 的 平面 放射 坐标 系 被 称 为 笛 卡 儿 坐 标 系 
(Cartesian Frame) ,如 果 两 条 数 轴 相互 垂直 , 则 
被 称 为 笛 卡 儿 直 角 坐 标 系 ,否则 被 称 为 笛 卡 儿 
斜 角 坐标 系 。 空 间 稍 卡 儿 直角 坐标 系 是 平面 笛 
卡 儿 坐标 系 向 三 维 空间 的 推广 ,具有 以 下 四 个 
特点 : 三 条 数 轴 交 于 原点 ; @ 三 条 数 轴 不 共 
面 ; @ 三 条 数 轴 度量 单位 相等 ; @@ 三 条 数 轴 相 图 1.50 笛 卡 儿 坐 标 系 与 物体 的 自由 度 
互 垂直 。 

2. 自由 度 (Degree of Freedom, DOF) 

自由 度 是 指 一 个 点 或 一 个 物体 运动 的 方式 或 一 个 动态 系统 的 变化 方式 ,也 指 描述 物体 
运动 所 需要 的 独立 坐标 数 或 独立 变量 数 。 每 个 自由 度 可 表示 一 个 独立 的 变量 ,而 利用 所 有 
的 自由 度 就 可 以 完整 描述 所 研究 物体 或 系统 的 位 置 和 姿态 。 描 述 三 维 空间 中 物体 (一 般 指 
刚体 ) 的 运动 通常 需要 6 个 变量 (z,y,z,a,B,7) ,前 3 个 变量 表示 物体 的 位 置 ,后 3 个 变量 表 
示 物 体 的 姿态 。 这 6 个 变量 是 基于 一 个 笛 卡 儿 直 和 角 坐 标 系 表示 的 ,如 图 1. 50 所 示 , 因 此 通 
常会 讲 一 个 自由 物体 在 三 维 空间 中 具有 6 个 自由 度 。 

3. 418 (Manipulator) 
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置 。 在 机 器 人 领域 ,通常 指 工业 机 械 辟 (工业 机 器 人 ) 或 其 他 类 型 的 机 器 人 臂 , 如 图 1.51 所 
示 。 目 前 操作 臂 主 要 有 两 种 结构 形式 : 一 体 化 结构 和 模块 化 结构 。 传 统 的 工业 机 器 人 多 采 
用 一 体 化 的 结构 ,有 一 个 集中 的 控制 柜 , 机 械 本 体 拆 开 就 不 能 工作 了 ; 而 模块 化 机 械 臂 的 每 
个 关节 是 一 个 集 电机 控制, 传 感 于 一 体 的 独立 结构 ,关节 模块 之 间 可 以 相互 通信 供电, 一般 
没有 一 个 集中 的 控制 柜 。 目 前 ,操作 臂 这 个 名 称 用 得 比较 少 了 ,逐渐 被 机 械 辟 (Arm) 所 代替 。 


笛 卡 儿 坐 标 系 Y 


(a) 工业 机 械 辟 (工业 机 器 人 )Esl (b) 模块 化 机 械 臂 中 9 
图 1.51 操作 臂 / 机 械 臂 


- 


4. 末端 执行 器 (End-Effector) 
末端 执行 器 是 机 器 人 执行 部 件 的 统称 , 它 一 般 位 于 机 器 人 腕 部 的 末端 ,是 直接 执行 工作 
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任务 的 装置 ,如 灵巧 手 、 夹 持 器 等 。 图 1.51(b) 中 所 示 机 械 臂 末端 的 三 指 机 械 手 就 属于 末端 
执行 器 。 

5. 手腕 (Wrist) 

手腕 是 机 器 人 的 某 个 或 某 几 个 关节 所 在 部 位 的 统称 ,起 到 类 似 人 的 手腕 的 作用 。 手 腕 
一 般 与 机 器 人 末端 执行 器 直接 连接 ,具有 支撑 和 调整 末端 执行 器 姿态 的 功能 。 手 腕 是 机 器 
人 操作 臂 /机 械 臂 的 重要 组 成 部 分 之 一 ,工业 机 器 人 的 后 三 个 关节 通常 起 到 手腕 的 作用 ,如 
图 1.51(a) 所 示 。 

6. 世界 坐标 系 (World Coordinate System) 

世界 坐标 系 一 般 是 指 建 立 在 地 球 上 的 笛 卡 儿 直 角 坐 标 系 ,也 被 称 为 大 地 坐标 系 。 该 坐 
标 系 相 对 于 地 球 上 的 其 他 物体 都 是 不 动 的 ,所 以 可 作为 通用 的 参考 系 。 

7. 基 座 坐标 系 (Base Coordinate System) 

基 座 坐标 系 也 被 称 为 基 坐 标 系 ,一 般 用 于 描述 机 器 人 操作 臂 , 是 指 建 立 在 机 器 人 不 运动 
的 基 座 上 的 坐标 系 , 该 坐标 系 相 对 于 机 器 人 的 其 他 部 分 是 静止 不 动 的 ,通常 用 作 描 述 机 器 人 
各 关节 运动 及 末端 位 姿 的 参考 坐标 系 。 

8. 坐标 变换 (Coordinates Transformation) 

坐标 变换 是 指 将 一 个 点 的 坐标 描述 从 一 个 坐标 系 转换 到 另 一 个 坐标 系 下 的 过 程 。 在 机 
器 人 运动 学 中 ,坐标 变换 非常 重要 ,通常 用 于 两 个 相 邻 连 杆 之 间 的 位 姿 转换 。 

9. 关节 空间 (Joint Space) 

关节 空间 是 机 器 人 关节 变量 所 构成 的 数学 意义 上 的 空间 集合 。 例 如 , 某 工 业 机 器 人 有 
6 个 关节 ,每 个 关节 位 置 用 变量 表示 为 0,i=1,2,…,6, 则 此 6 个 关节 变量 可 构成 一 个 关节 
空间 集合 。 此 外 ,机 器 人 的 关节 位 置 变量 .关节 速度 变量 和 关节 加 速度 变量 都 可 独立 或 组 合 
构成 机 器 人 的 关节 空间 。 

10. 工作 空间 (Working Space) 

机 器 人 工作 空间 有 两 层 含义 。 一 层 是 数学 意义 上 的 , 指 的 是 机 器 人 工作 空间 变量 所 构 
成 的 空间 集合 。 例 如 , 某 6 自由 度 工 业 机 器 人 的 工作 空间 可 用 6 个 变量 ryza py 描述 ， 
这 6 个 变量 可 构成 机 器 人 的 工作 空间 。 另 一 层 含 义 是 几何 层面 的 ,是 指 机 器 人 运动 描述 参 
考点 所 能 到 达 的 空间 点 的 集合 ,一般 只 考虑 机 器 人 工作 空间 的 位 置 变量 ,如 xz,y,z, 通 常用 
于 描述 机 器 人 操作 臂 的 工作 范围 , 它 是 由 操作 臂 的 连 杆 尺寸 .关节 运动 范围 和 构 型 决定 的 。 
机 器 人 工作 空间 的 形状 因 机 构 类 型 不 同 而 不 同 , 例 如 直角 坐标 机 器 人 的 工作 空间 是 一 个 拢 
形 六 面体 ,圆柱 坐标 机 器 人 的 工作 空间 是 一 个 开口 空心 圆柱 体 , 极 坐标 机 器 人 的 工作 空间 是 
一 个 空心 球体 。 因 为 机 器 人 的 转动 关节 受 结构 的 限制 ,一 般 不 能 整 圈 转 动 ,所 以 关节 式 机 器 
人 的 工作 空间 比较 复杂 ,图 1. 52 所 示 的 是 某 6 自由 度 关 节 式 机 器 人 的 工作 空间 投影 图 。 

11. 额定 负载 (Rated Load) 

额定 负载 是 指 机 器 人 在 规定 的 性 能 范围 内 末端 机 械 接 口 处 能 够 承受 的 最 大 负载 量 。 该 
指标 反映 了 机 器 人 搬运 重 物 的 能 力 , 通 常用 来 表示 机 械 臂 的 承载 力 。 

12. 分 辩 率 (Resolution) 

分 辨 率 是 指 机 器 人 每 个 关节 能 够 实现 的 最 小 移动 距离 或 最 小 转动 角度 。 该 指标 反映 了 
机 器 人 关节 传感器 的 检测 精度 及 关节 的 运动 精度 。 
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(a) 侧 视图 (b) 俯视 图 


图 1.52 茶 关节 式 机 器 人 的 工作 空间 


13. 定位 精度 (Positioning Accuracy) 
定位 精度 是 指 机 器 人 执行 指令 设 定位 姿 与 实际 到 达 位 姿 的 一 致 程度 。 在 机 器 人 的 技术 指 
标 中 ,定位 精度 通常 用 重复 定位 精度 来 表示 ,例如 某 工 业 机 器 人 的 重复 定位 精度 为 士 0.01mrmna。 
14. 点 位 控制 (Point to Point Control, PTP) 
点 位 控制 是 机 器 人 的 一 种 典型 控制 方式 ,控制 机 器 
人 从 一 个 位 姿 运 动 到 下 一 个 位 姿 , 只 保证 起 点 和 终点 处 
位 瓷 的 准确 性 ,不 限定 其 中 间 的 过 渡 路 径 , 由 机 器 人 的 控 
制 器 和 驱动 器 自动 选择 。 例 如 ,在 点 焊 和 物品 搬运 中 ,可 
采用 点 位 控制 让 机 器 人 顺利 地 通过 和 到 达 某 些 点 ,而 不 
必 限 定 两 个 点 之 间 的 运动 轨迹 的 形状 。 如 图 1.53 所 示 
是 机 器 人 某 关节 的 点 位 运动 曲线 ,图 中 的 圆 点 是 必须 经 
过 的 ,而 两 个 相 邻 圆 点 之 间 的 轨迹 可 以 是 任意 形状 的 。 
15. 连续 轨迹 控制 (Continuous Path Control. CP) 
连续 轨迹 控制 是 一 种 比 点 位 控制 更 复杂 的 控制 方式 , 它 能 控制 机 器 人 的 机 械 接口 在 指 
定 的 轨迹 上 按照 编程 规定 的 位 姿 和 速度 移动 。 例 如 ,在 如 图 1.54 所 示 的 机 器 人 弧 焊 中 , 需 
要 采用 连续 轨迹 控制 而 不 是 点 位 控制 ,让 机 器 人 带 着 焊 炬 完成 预定 的 连续 焊 缝 的 焊接 作业 。 
如 果 采 用 的 是 点 位 控制 , 则 无 法 保证 焊 颖 的 形状 ,自然 无 法 达到 预定 的 焊接 质量 。 


0 


图 1.53 机 器 人 点 位 控制 


图 1.54 机 器 人 弧 焊 所 
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16. 协调 控制 (Coordinated Control) 
协调 控制 是 对 多 个 机 器 人 而 言 的 ,该 控制 方式 可 以 协调 多 个 手臂 或 多 台 机 器 人 同时 进 
行 某 种 作业 。 例 如 ,如 图 1. 55 所 示 的 4 个 机 械 臂 协调 合作 完成 作业 任务 。 目 前 ,机 器 人 的 


图 1.55 多 机 器 人 协调 作业 经 


17. 伺服 系统 (Servo System) 

伺服 系统 是 控制 机 器 人 的 位 次 和 速度 等 使 其 跟随 目标 值 变化 的 控制 系统 。 何 服 系统 是 
机 器 人 的 控制 核心 ,目前 主要 有 基于 工控 机 的 伺服 系统 和 基于 从 入 式 控 制 器 的 伺服 系统 两 
大 类 。 基 于 工控 机 的 伺服 系统 常用 于 工业 机 器 人 等 大 功率 机 器 人 系统 ,基于 其 入 式 控制 器 
的 伺服 系统 通常 用 于 移动 机 器 人 等 小 型 机 器 人 系统 。 

18. 离线 编程 (Off-line Programming) 

离线 编程 是 机 器 人 作业 方式 的 信息 记忆 过 程 与 作业 对 象 不 发 生 直 接 关系 的 编程 方式 。 
例如 ,在 计算 机 上 编写 机 器 人 的 控制 程序 ,然后 让 机 器 人 按 着 编写 的 程序 运动 ,这 种 编程 方 

19. 在 线 编程 (On-line Programming) 

在 线 编 程 是 让 机 器 人 在 执行 任务 的 过 程 中 记忆 下 运动 参数 及 轨迹 的 一 种 编程 方式 , 目 
前 这 种 编程 方式 的 应 用 比较 少 。 在 线 编程 最 常用 的 方式 是 人 工 示 教 , 即 一 个 操作 熟练 的 人 
牵引 机 器 人 的 工具 完成 操作 任务 ,机 器 人 在 运动 过 程 中 记忆 下 运动 参数 并 能 够 复 现 整个 运 
动 轨迹 ,如 图 1.56 所 示 。 在 喷漆 机 器 人 的 编程 中 常 采 用 人 工 示 教 的 在 线 编程 方式 。 


图 1.56 HARA” 


第 1 章 绪论 |P 23 


20. 机 器 人 语言 (Robot Language) 

在 机 器 人 的 早期 阶段 ,需要 采用 专用 的 计算 机 编程 语言 编写 机 器 人 的 控制 程序 ,主要 有 
VAL.VAL2.LAMA.RAIL 等 。 目 前 ,大 多 数 机 器 人 的 编程 语言 是 采用 主流 的 计算 机 程序 
IE m C. CHCH, 

21. 传感器 (Sensors) 

机 器 人 采用 传感器 感知 自己 和 周围 环境 ,因此 机 器 人 的 传 感 融 主要 分 为 内 部 传感器 
(internal sensors) 和 外 部 传感器 (external sensors) 。 例 如 ,检测 机 器 人 关节 运动 的 编码 器 
属于 内 部 传感器 ,检测 与 物体 之 间接 触 力 信息 的 属于 外 部 传感器 。 机 器 人 的 外 部 传感器 主 
要 有 力 传 感 器 .超声 传感器 .激光 传感器 .视觉 传感器 等 ,如 图 1. 57 所 示 。 


DS 


T DEERE 


机 器 视觉 


图 1.57 机 器 人 中 的 传感器 


1.4 机 器 人 的 分 


目前 ,机 器 人 的 功能 多 种 多 样 ,外形 也 千奇百怪 ,对 现 有 机 器 人 进行 分 类 不 是 一 件 容 易 
的 事情 。 依 据 国 际 机 器 人 联合 会 CIFR) 的 分 类 , 目前 的 机 器 人 主要 分 为 工业 机 器 人 
(Industrial Robot) 和 服务 机 器 人 (Service Robot) 两 大 类 。 这 种 分 类 主要 是 依据 机 器 人 的 应 
用 领域 及 服务 对 象 来 划分 的 。 

工业 机 器 人 主要 用 于 生产 制造 流通 领域 ,应 用 场合 是 工厂 ,用 途 包括 焊接 、 铸 造 、 喷 涂 、 
冲压 装配、 搬运 等 ,工业 机 器 人 在 不 同 应 用 中 的 占 比 如 图 1. 58 所 示 。 工 业 机 器 人 的 主要 结 
构 形 式 为 多 自由 度 机 械 臂 (如 图 1. 59 所 示 ) 和 轮 式 移动 机 器 人 (如 图 1. 60 所 示 ) ,其 中 轮 式 
移动 机 器 人 主要 包括 常规 轮 式 移动 机 器 人 和 全 向 移动 机 器 人 (麦克 纳 姆 轮 ) B 2013 年 起 ， 
中 国 市 场 销售 的 工业 机 器 人 数量 连续 排名 世界 第 一 ,超越 了 日 本 .美国 等 发 达 国 家 ,但 中 国 
制造 业 中 机 器 人 数量 与 工人 数量 的 比值 还 很 低 , 远 远 低 于 美国 日本、 德国、 韩国 等 国家 。 

服务 机 器 人 主要 为 人 类 提供 最 直接 的 服务 或 替代 ,主要 包括 专业 服务 机 器 人 
(Professional Service Robots), 个 人 及 家 用 服务 机 器 人 (Service Robots for Personal and 
Domestic Use) 两 大 类 。 专 业 服 务 机 器 人 主要 包括 农 牧 机 器 人 、 食 品 机 器 人 、 建 筑 机 器 人 、 救 
援 机 器 人 、 医 疗 机 器 人 .军用 机 器 人 等 ,如 图 1.61 所 示 。 个 人 及 家 用 服务 机 器 人 主要 包括 玩 
具 机 器 人 清洁 机 器 人 、 陪 护 机 器 人 教育 机 器 人 等 ,如 图 1. 62 所 示 。 目 前 ,服务 机 器 人 的 应 
用 范围 及 类 型 远 比 工业 机 器 人 多 得 多 。 
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图 1.58 工业 机 器 人 在 不 同 应 用 中 的 占 比 


(a) 多 自由 度 机械 六 焊接 装配 ppa (b) 多 自由 度 机 械 臂 喷涂 531 
图 1.59 多 自由 度 机 械 臂 


(a) 常规 轮 式 移动 机 器 人 [9 (b) 全 向 移动 机 器 人 BE71 
图 1.60 轮 式 移动 机 器 人 


挤 奶 机 器 人 9 破 拆 机 器 人 [3 机 器 人 宇航 员 的 军用 机 器 人 [1 
图 1.61 专业 服务 机 器 人 
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MAHLAS HAWLA 护理 机 器 人 [9 扫地 机 器 人 [的 
图 1.62 个 人 及 家 用 服务 机 器 人 


1.5 机 器 人 的 主要 研究 方向 


机 器 人 学 是 多 学 科 交 叉 的 新 兴 前 沿 学 科 ,涉及 机 械 学 .生物 学 .计算 机 科学 与 工程 、 控 制 
理论 与 控制 工程 .电子 工程 、 人 工 智 能 .人 类 学 .社会 学 等 。 简 而 言 之 ,机 器 人 学 是 研究 机 顺 
人 的 科学 和 技术 的 学 科 。 目 前 ,机 器 人 的 主要 研究 方向 有 机 器 人 机 构 、 机 器 人 运动 学 、 机 器 
人 动力 学 .机 器 人 控制 .机 器 人 感知 等 。 下 面 对 各 研究 方向 做 简要 介绍 。 

1. 机 器 人 机 构 (Mechanism) 

机 器 人 机 构 是 用 来 将 输入 的 运动 和 力 转换 成 期 望 的 力 和 运动 的 输出 。 机 器 人 机 构 按 工 
作 空 间 可 分 为 平面 机 构 和 空间 机 构 , 按 刚度 可 分 为 刚性 机 构 和 和 柔性 机 构 。 机 器 人 机 构 主 要 
研究 机 构 的 构 型 尺度. 速度、 负载 能 力 及 机 构 的 刚度 。 

2. 机 器 人 感知 (Sensing) 

机 器 人 感知 是 通过 不 同 的 传感器 来 实现 的 ,主要 包括 内 部 传感器 和 外 部 传感器 两 大 类 。 
机 器 人 感知 主要 研究 机 器 人 专用 传感器 的 研制 及 相关 的 传 感 信息 处 理 方 法 和 技术 。 

3. 机 器 人 运动 学 (Kinematics) 

机 器 人 运动 学 主要 研究 机 器 人 的 人 位置、 速度、 加 速度 及 其 他 位 置 变量 的 高 阶 导数 ,包括 
正 运 动 学 (Forward Kinematics) 和 道 运动 学 (Inverse Kinematics) 两 大 类 问题 。 运 动 学 研究 
机 器 人 的 运动 ,但 不 考虑 产生 运动 的 力 。 

4. 机 器 人 动力 学 (Dynamics) 

机 器 人 动力 学 是 研究 机 器 人 产生 预定 运动 需要 的 力 , 如 关节 电机 了 驱动 絮 输 出 的 力矩 。 
机 器 人 动力 学 的 基础 是 牛顿 力学 (Newtonian Mechanics )、 拉 格 并 日 力学 (Lagrangian 
Mechanics) 和 汉密尔顿 力学 (Hamiltonian Mechanics). i 

5. 机 器 人 控制 (Control) 

机 器 人 控制 以 机 器 人 运动 学 和 动力 学 为 基础 ,主要 包括 位 置 控制 (Position Control), J 
控制 (Force Control) 、 力 位 混合 控制 (Hybrid Control) 等 类 型 。 机 器 人 的 智能 是 由 其 控制 
系统 和 控制 方法 来 体现 。 


1.6 人 小 结 


` 


本 章 首先 介绍 了 机 器 人 的 定义 ,然后 分 阶段 介绍 了 机 器 人 的 发 展 历程 ,并 对 我 国 机 器 人 
的 发 展 进行 了 概述 ,在 此 基础 上 介绍 了 机 器 人 中 重要 的 专业 术语 及 机 器 人 的 分 类 ,最 后 介绍 
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了 机 器 人 的 主要 研究 方向 。 本 章 内 容 既 能 帮助 读者 了 解 机 器 人 的 发 展 历史 及 现状 ,又 能 为 
读者 学 习 后 续 章 节 的 内 容 提供 必要 的 基础 知识 储备 。 š 
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机 器 人 机 构 


CHAPTER 2 


机 构 是 机 器 的 某 种 抽象 表示 形式 ,有 两 个 核心 组 成 要 素 : 构件 和 运动 副 。 机 器 人 机 构 
的 主要 作用 是 实现 运动 的 传递 及 预期 的 运动 形式 ,是 机 器 人 智能 实现 和 展示 的 载体 。 虽 然 
目前 世界 上 有 各 种 各 样 的 机 器 人 ,但 是 其 机 构 类 型 是 很 有 限 的 。 本 章 首 先 介绍 机 器 人 机 构 
的 分 类 及 特点 ,然后 介绍 机 器 人 机 构 简 图 的 绘制 方法 ,最 后 重点 介绍 5 种 典型 串联 机 器 人 机 
构 和 3 种 典型 并 联机 融 人 机 构 。 


2.1 机 构 的 基础 知识 


机 构 是 人 们 在 实践 中 发 明 的 用 于 改造 和 适应 自然 的 工具 ,有 了 人 就 有 了 机 构 的 创造 , 杠 
杆 、 车 轮 等 都 是 人 类 创造 的 简单 实用 机 构 , 如 图 2.1 和 图 2. 2 所 示 。 


图 2.1 杠杆 机 构 站 图 2.2 EPA 


机 构 有 明确 的 定义 : 机 构 是 由 原 动 件 和 机 架 组 成 的 ,具有 确定 运动 的 运动 链 。 图 2. 3 
所 示 的 是 大 家 熟知 的 平面 四 连 杆 机 构 简 图 ,该 机 构 是 由 4 个 构件 和 4 个 转动 副 组 成 ,只 有 
1 个 自由 度 。 

研究 机 构 的 学 科 被 称 为 机 构 学 ,是 机 械 原理 的 重要 分 支 。 
1875 年 ,德国 学 者 勒 洛 (F，Reuleau) 最 先 给 出 了 机 构 学 的 定 
义 : 机 构 学 是 研究 机 器 的 组 成 (构件 和 运动 副 ) .运动 原理 以 及 ， 
分 析 设计 方法 的 科学 。 例 如 ,机 构 学 研究 各 种 常用 机 构 的 结构 
和 运动 ,如 连 杆 机 构 .凸轮 机 构 ,齿轮 机 构 . 差 动 机 构 .间歇 运动 4 
机 构 、 直 线 运动 机 构 、 螺 旋 机 构 等 。 根 据 机 构 中 各 构件 的 相对 ”图 2.3 平面 四 连 杆 机 构 
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运动 是 在 二 维 平面 内 还 是 在 三 维 空间 中 ,可 将 机 构 分 为 平面 机 构 和 空间 机 构 两 大 类 。 绝 大 
部 分 机 器 人 机 构 属 于 空间 机 构 , 少 部 分 属于 平面 机 构 。 


2.2 机 器 人 机 构 分 类 


对 机 器 人 机 构 进 行 分 类 并 不 是 一 件 容 易 的 事情 ,在 本 书 中 ,为 了 便于 介绍 ,将 机 器 人 机 
构 分 为 三 大 类 : 串联 机 器 人 机 构 .并 联机 器 人 机 构 和 其 他 机 器 人 机 构 。 


2.2.1 串联 机 器 人 机 构 


运动 链 是 由 两 个 或 两 个 以 上 的 构件 通过 运动 副 的 连接 而 构成 的 具有 相对 运动 的 系统 。 
运动 链 主 要 有 两 种 类 型 : 开 式 链 、 闭 式 链 。 机 器 人 机 构 本 质 上 也 是 由 运动 链 构 成 的 ,是 一 种 
具有 机 架 的 运动 链 。 

串联 机 器 人 机 构 (Serial Robot Mechanism) 是 从 基 座 开始 由 连 杆 和 关节 顺序 连接 而 构 
成 的 开 式 链 机 构 。— 典 型 的 例子 有 6 自由 度 的 工业 机 器 人 、4 自由 度 的 码 埃 机 器 人 4 自由 度 
的 SCARA 机 器 人 等 ,如 图 2.4 所 示 , 构 成 这 些 机 器 人 机 构 的 运动 链 都 是 开 式 链 。 我 们 通常 
所 说 的 串联 机 器 人 是 从 机 构 类 型 的 角度 来 称呼 的 。 

串联 机 器 人 机 构 具 有 下 列 优 缺 点 。 

优点 : 工作 空间 大 ,运动 速度 快 ,正解 计算 比较 简单 。 

缺点 : 刚度 较 弱 ,定位 精度 较 低 , 逆 解 计算 复杂 。 


(a) 6 自由 度 工业 机 器 人 站 (b) 4 自由 度 码 埃 机 器 人 钙 (c) 4 自由 度 装 配 机 器 人 加 
图 2.4 串联 机 器 人 


2.2.2 并 联机 器 人 机 构 


并 联机 构 (Parallel Mechanism) 是 动 平 台 和 静 平 台 通过 至 少 两 条 独立 的 运动 链 相 连接 ， 
具有 两 个 或 两 个 以 上 自由 度 且 以 并 联 方式 驱动 的 一 种 闭 链 机 构 。 并 联机 器 人 (Parallel 
Robot) ,顾名思义 就 是 采用 并 联机 构 的 机 器 人 。 典 型 的 并 联机 器 人 机 构 包 括 6 自由 度 的 
Stewart 机 构 、3 自由 度 的 Delta 机 构 , 如 图 2.5 所 示 。 

并 联机 器 人 机 构 具 有 如 下 优 缺 点 。 

优点 : 刚度 大 ,定位 精度 高 , 逆 解 计算 简单 。 

缺点 : 工作 空间 小 ?运动 速度 低 ,正解 计算 复杂 。 

需要 说 明 的 是 ,前 面 提 到 的 串联 机 器 人 机 构 和 并 联机 器 人 机 构 的 优 缺 点 是 以 两 种 机 构 
对 比 得 出 的 结果 ,比如 这 里 讲 并 联机 器 人 的 运动 速度 低 , 是 相对 于 串联 机 器 人 而 言 的 , 它 实 
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(a) 6 自由 度 Stewart 机 构 (b) 31 Hi JE Delta J Je! 
图 2.5 并 联机 器 人 机 构 


际 的 运动 速度 是 很 快 的 。 


2.2.3 其 他 机 器 人 机 构 


串联 机 器 人 机 构 和 并 联机 器 人 机 构 一 般 被 称 为 典型 机 器 人 机 构 , 除 此 以 外 ,还 有 一 些 其 
他 机 器 人 机 构 , 例 如 移动 机 器 人 机 构 、 飞 行 机 器 人 机 构 等 ,如 图 2.6 和 图 2.7 所 示 ,它们 主要 
是 由 连 杆 机 构 .齿轮 机 构 等 传统 机 构 构 成 。 这 些 机 器 人 机 构 大 多 来 自 于 过 去 已 有 的 机 器 系 
统 , 最 主要 的 变化 是 其 控制 由 过 去 的 人 工 操 控 变 为 自动 控制 。 


图 2.6 移动 机 器 人 中 图 2.7 飞行 机 器 人 


机 器 人 机 构 可 能 由 单一 机 构 类 型 构成 ,也 可 能 是 多 种 机 构 类 型 的 组 合 。 例 如 ,如 图 2.8 
所 示 的 AIBO 机 器 狗 是 由 串联 机 构 和 其 他 机 构 组 合 而 成 。 


图 2.8 AIBO LEM 
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2.3 机 器 人 机 构 简 图 画 法 


对 于 图 2. 8 中 的 机 器 人 ,读者 能 从 图 中 看 出 它 的 自由 度数 .关节 类 型 及 关节 之 间 的 连接 
关系 吗 ? 如 果 读者 不 熟悉 这 个 机 器 人 ,很 显然 答案 是 否定 的 。 那 么 能 否 采 用 一 种 直观 的 形 
式 表 示 机 器 人 机 构 信 息 呢 ? 答案 是 肯定 的 ,就 是 机 器 人 机 构 简 图 。 机 器 人 的 机 构 简 图 是 采 
用 简单 的 图 形 符号 来 表示 机 器 人 机 构 。 

在 绘制 机 器 人 机 构 简 图 时 ,应 首先 确定 表示 机 器 人 机 构 的 图 形 符号 ,然后 再 根据 机 器 人 
机 构 的 具体 组 成 及 连接 关系 进行 绘制 。 


2.3.1 典型 机 器 人 机 构 的 图 形 符号 


串联 机 器 人 机 构 与 并 联机 器 人 机 构 属 于 典型 机 器 人 机 构 , 如 图 2.4 和 图 2.5 所 示 ,它们 
的 机 构 主 要 由 关节 和 连 杆 组 成 。 因 此 可 用 表示 关节 和 连 杆 的 图 形 符号 来 表示 典型 机 器 人 的 
机 构 , 即 画 出 机 器 人 的 机 构 简 图 。 

运动 副 是 两 构件 直接 接触 并 能 产生 相对 运动 的 活动 连接 ,主要 包括 高 副 和 低 副 两 大 类 。 
构成 高 副 的 两 构件 之 间 是 线 接触 或 点 接触 ,接触 压强 比较 高 ,如 凸轮 与 从 动 件 、 两 齿轮 传动 
等 。 构 成 低 副 的 两 构件 之 间 是 面 接触 ,接触 压强 比较 低 , 如 旋转 副 移动 副 、 圆 柱 副 等 。 由 于 
高 副 一 般 比 低 副 容易 磨损 ,因此 机 器 人 中 的 运动 副 一 般 都 是 低 副 。 

在 机 器 人 中 ,机 器 人 的 关节 就 是 运动 副 ,主要 包括 两 类 关节 : 转动 关节 和 移动 关节 ,分 
别 如 图 2.9 和 图 2. 10 所 示 。 和 转动 关节 是 一 个 构件 (转动 件 ) 相 对 于 另 一 个 构件 ( 基 座 ) 绕 匀 
接 的 转轴 产生 相对 旋转 运动 。 移 动 关 节 是 一 个 滑动 构件 相对 于 一 个 固定 构件 沿 直 线 导 路 产 
生 相 对 移动 。 这 两 类 关节 都 只 有 一 个 自由 度 。 


基 座 , P 


固定 件 


ra 
. 


滑动 件 
图 2.9 转动 关节 图 2.10 移动 关节 


在 典型 机 器 人 机 构 简 图 的 绘制 中 ,会 用 到 如 表 2. 1 所 示 的 图 形 符 号 。 需 要 说 明 的 是 , 目 
前 机 器 人 机 构图 形 符号 的 表示 方式 还 没有 统一 ,而 且 这 些 图 形 符号 只 是 一 种 象形 表达 ,因此 
对 于 同样 的 机 器 人 机 构 ,在 不 同 的 资料 中 会 有 不 同 的 图 形 符号 表示 方式 。 
表 2.1 典型 机 构图 形 符号 
名 称 图 形 符 号 
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续 表 
名 K HERFS ` 

球 关 节 m d 

圆柱 关节 = 2 

连 杆 一 一 一 

未 端 执行 器 — 

n PR an 


ER 2. 1 中 ,移动 关节 有 四 种 图 形 符号 : 前 两 种 都 表示 矩形 滑 块 可 沿 平行 于 纸 面 的 轴 
线 移动 ,只 是 滑 块 的 画 法 不 同 ; 第 三 种 图 形 符号 表示 矩形 滑 块 可 沿 垂直 于 纸 面 的 轴线 移动 ; 
第 四 种 符号 多 用 于 表示 电动 币 ,液压 币 等 驱动 的 直线 移动 关节 ,在 并 联机 器 人 机 构 中 这 种 符 
号 使 用 最 多 。 

转动 关节 也 有 四 种 图 形 符号 : 第 一 种 表示 绕 垂直 于 纸 面 的 轴线 转动 的 关节 ; 第 二 ,三 
种 表示 绕 平行 于 纸 面 的 轴线 转动 的 关节 ; 第 四 种 也 表示 绕 平 行 于 纸 面 的 轴线 转动 的 关节 ， 
但 其 画 法 更 加 简洁 。 

球 关 节 有 一 种 图 形 符号 ,表示 圆 球 可 在 球 窝 中 任意 转动 。 球 关节 也 就 是 球 副 , 有 3 个 转 
动 自由 度 。 

圆柱 关节 有 一 种 图 形 符号 ,表示 该 关节 可 以 绕 中 间 的 轴线 转动 和 移动 ,圆柱 关节 也 就 是 
圆柱 副 , 有 2 个 自由 度 。 

连 杆 的 表示 很 简单 ,用 直线 段 表示 。 末 端 执 行 器 采用 的 是 象形 的 表示 方法 ,用 一 个 渔 又 
状 表示 机 器 人 末端 安装 的 机 械 手 等 执行 器 。 

基 座 是 工业 机 融 人 必 不 可 少 的 组 成 部 分 ,一 般 表 示 机 咒 人 固 连 于 地 面 , 因 此 用 剂 面 线 表 
示 地 面 。 

在 2.4 节 中 将 采用 上 述 图 形 符号 绘制 机 器 人 的 机 构 简 图 。 


2.3.2 其 他 机 器 人 机 构 的 简 图 画 法 


对 于 其 他 机 器 人 的 机 构 简 图 一 般 采 用 “机 械 原理 ”课程 中 学 过 的 机 构 简 图 的 画 法 或 象形 
画 法 ,主要 表示 出 其 运动 机 构 的 组 成 及 运动 类 型 。 例 如 ,如 图 2. 11(a) 所 示 的 扫地 机 器 人 ， 


(a) 扫地 机 器 人 100 (b) 机 构 简 图 
图 2.11 扫地 机 器 人 及 其 机 构 简 图 
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其 运动 底盘 机 构 由 两 个 独立 驱动 的 轮子 ( 单 向 轮 ) 和 一 个 随 动 轮 ( 万 向 轮 ) 组 成 ,其 机 构 简 图 
如 图 2. 11(b) 所 示 ; 如 图 2. 12(a) 所 示 的 四 轮 移 动机 器 人 ,其 运动 底盘 机 构 是 由 4 个 独立 驱 
动 的 轮子 组 成 ,其 机 构 简 图 如 图 2. 12(b) 所 示 。 


(a) 四 轮 移 动机 器 人 1 (b) 机 构 简 图 
图 2.12 四 轮 移动 机 器 人 及 其 机 构 简 图 


2.4 典型 机 器 人 机 构 介 绍 


本 节 主 要 介绍 串联 机 器 人 ,并 联机 器 人 这 两 大 类 典型 机 器 人 所 包含 的 不 同 机 器 人 机 构 
类 型 .用途 和 特点 ,并 画 出 其 机 构 简 图 , 


2.4.1 串联 机 器 人 机 构 介 绍 


从 机 构 类 型 上 区 分 ,串联 机 器 人 可 分 为 五 种 : 直角 坐标 机 器 人 、 圆 柱 坐 标 机 器 人 、 球 坐 
标 机 器 人 、SCARA 机 器 人 、 通 用 多 关节 型 机 器 人 。 下 面 对 各 种 类 型 的 串联 机 器 人 的 机 构 、 
特点 和 用 途 进行 介绍 并 给 出 其 机 构 简 图 。 

1) 直角 坐标 机 器 人 

直角 坐标 机 器 人 也 被 称 为 笛 卡 儿 机 器 人 (Cartesian Robot) 或 X YZ 机 器 人 ,是 指 机 器 人 
的 三 个 关节 分 别 沿 着 直角 坐标 系 的 X.Y .Z 三 个 坐标 轴 做 直线 运动 ,由 此 在 末端 产生 可 控 
运动 轨迹 和 任意 点 的 定位 ,如 图 2. 13(a) 所 示 。 直 角 坐 标 机 器 人 主要 有 龙门 结构 、 壁 挂 结 
构 、 垂 挂 结构 ,多 用 于 物品 的 搬运 、 工 件 加 工 和 3D 打印 等 。 

根据 表 2. 1 中 的 机 构图 形 符号 ,可 画 出 图 2.13(a) 中 的 直角 坐标 机 器 人 的 机 构 简 图 ,如 


y 


— 


= 


(a) 直 前 坐标 机 器 人 (b) 机 构 简 图 
图 2.13 直角 坐标 机 器 人 及 其 机 构 简 图 
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图 2. 13(b) 所 示 。 从 该 机 构 简 图 可 以 清楚 地 看 出 ,该 机 器 人 是 由 3 个 正 交 垂直 的 移动 关节 
组 成 ,其 末端 的 位 置 可 表示 为 P 二 F(X,Y,2), 即 关节 变量 是 3 个 移动 变量 。 

机 器 人 的 工作 空间 是 机 器 人 正常 运动 时 末端 坐标 系 原 点 能 到 达 的 空间 点 的 集合 , 即 能 
覆盖 的 空间 ,又 称 可 达 工 作 空 间或 总 工作 空间 。 工 作 空 间 是 表示 机 器 人 运动 范围 的 一 个 重 
要 参数 。 

直角 坐标 机 器 人 的 工作 空间 如 图 2. 14 所 示 。 从 图 中 可 以 看 出 ,直角 坐标 机 器 人 的 工作 
空间 是 一 个 规则 的 长 方 体 , 它 的 长 、 宽 、 高 分 别 对 应 机 器 人 在 XYZ 三 个 方向 的 运动 范围 。 


RE — l EA 


图 2.14 直角 坐标 机 器 人 的 工作 空间 


直角 坐标 机 器 人 具有 下 列 特点 : 

(1) 三 个 移动 关节 的 运动 相互 独立 ,因此 运动 学 建 模 及 反 解 简单 ; 

(2) 运动 速度 快 .定位 精度 高 ; 

(3) 结构 简单 ,控制 容易 ; 

(4) 移动 副 可 两 端 支 撑 , 结 构 刚 性 大 ; 

(5) 占据 空间 大 。 

问题 1: 第 一 台 直 角 坐 标 机 器 人 是 哪 年 研制 出 来 的 ? 

2) 圆柱 坐标 机 器 人 

圆柱 坐标 机 器 人 是 由 一 根 立 柱 和 安装 在 立柱 上 的 水 平 臂 组 成 ,立柱 安装 在 基 座 上 ,水平 
臂 可 绕 立柱 做 旋转 运动 .可 伸缩 并 沿 立 柱 上 下 移动 ,如 图 2. 15(a) 所 示 , 因 此 该 机 器 人 具有 
一 个 转动 关节 和 两 个 移动 关节 。 该 类 机 器 人 被 称 为 圆柱 坐标 机 器 人 是 因为 其 末端 可 到 达 的 
工作 空间 的 形状 是 一 个 空心 的 圆柱 。 圆 柱 坐 标 机 器 人 可 用 于 物品 的 搬运 ,喷涂 ,焊接 等 领域 。 

图 2. 15(a) 中 的 圆柱 坐标 机 器 人 的 机 构 简 图 有 两 种 画 法 : 一 种 是 采用 两 个 独立 的 移动 
副 和 一 个 转动 副 绘制 机 构 简 图 ,如 图 2. 15(b) 所 示 ; 另 一 种 是 将 机 器 人 的 前 两 个 关节 用 圆 
柱 副 来 表示 ,再 加 一 个 移动 副 来 绘制 机 构 简 图 ,如 图 2. 15(c) 所 示 。 第 二 种 机 构 简 图 更 简 
洁 , 但 第 一 种 机 构 简 图 更 符合 机 器 人 的 实际 结构 ,因为 目前 机 器 人 的 关节 一 般 只 有 1 个 自由 
度 ,还 未 出 现 具有 2 个 以 上 自由 度 的 独立 关节 。 圆 柱 坐 标 机 器 人 末端 的 位 置 可 表示 为 P = 
F(Z,0,R), 即 关节 变量 包括 1 个 转动 变量 和 2 个 移动 变量 。 

圆柱 坐标 机 器 人 工作 空间 的 形状 如 图 2. 16 所 示 。 从 图 中 可 以 看 出 ,该 机 器 人 在 水 平面 
的 工作 空间 很 大 ,这 主要 得 益 于 腰 转 关节 。 
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(a) 圆柱 坐标 机 器 人 (b) 机 构 简 图 1 (e) 机 构 简 图 2 


图 2.15 圆柱 坐标 机 器 人 及 其 机 构 简 图 


1 最 大 工作 半径 ' OKERRIK 。 ; 
1 I 1 
| 水 平行 程 ， 

1 I 


正视 图 


2.16 圆柱 坐标 机 器 人 的 工作 空间 


圆柱 坐标 机 器 人 的 特点 包括 : 

(1) 三 个 关节 的 运动 耦合 性 较 弱 ,运动 学 建 模 及 反 解 较 简单 ; 

(2) 运动 灵活 性 较 好 ,能 够 伸 人 型 腔 式 结构 内 部 ; 

(3) 手臂 可 达 空 间 受 限 ,不 能 到 达 靠 近 立 柱 或 地 面 的 空间 

(4) 结构 较 大 ,自身 占据 空间 也 较 大 。 

问题 2: 第 一 台 圆 柱 坐 标 机 器 人 是 哪 年 研制 出 来 的 ? 它 的 名 字 是 什么 ? 

3) 球 坐 标 机 器 人 

球 坐 标 机 器 人 也 被 称 为 极 坐标 机 器 人 ,由 腰 转 关节 、 俯 仰 关 节 和 伸缩 臂 组 成 , 共 3 个 自 
H FEE ,如 图 2.17(a) 所 示 , 因 其 末端 工作 空间 的 几何 形状 为 球面 ,因此 被 称 为 球 坐标 机 器 人 。 
球 坐 标 机 器 人 可 用 于 物品 的 搬运 .喷涂 .焊接 等 领域 。 
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(b) 机 构 简 图 


(a) 球 坐 标 机 器 人 
图 2.17 球 坐 标 机 器 人 及 其 机 构 简 图 
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图 2. 17(a) 所 示 的 球 坐 标 机 器 人 的 机 构 简 图 如 图 2. 17(b) 所 示 。 球 坐标 机 器 人 末端 的 
位 置 可 表示 为 PFO. $.R) , 即 关 节 变 量 包括 2 个 转动 变量 和 1 个 移动 变量 。 

球 坐 标 机 器 人 工作 空间 的 形状 如 图 2. 18 所 示 。 从 图 中 可 以 看 出 ,该 类 机 器 人 在 水 平面 
上 具有 较 大 的 工作 空间 ,但 在 垂直 平面 内 的 工作 空间 比较 小 。 


水 平 极限 伸 长 ， ， 水 平行 程 ， 
"— M ka—an 


wam 
水 平行 程 | 
— 


垂直 极限 升 高 


图 2.18 球 坐 标 机 器 人 的 工作 空间 


球 坐 标 机 器 人 的 特点 包括 : 

(1) 三 个 关节 的 运动 耦合 性 强 ,运动 学 建 模 和 逆 解 复杂 ; 

(2) 工作 空间 较 大 ; 

(3) 运动 灵活 性 好 ,自身 占据 空间 小 ; 

(4) 存在 工作 死 区 ,控制 复杂 。 

问题 3: 第 一 台球 坐标 机 器 人 的 名 字 是 什么 ? 

4) SCARA 机 器 人 

SCARA(Selective Compliance Assembly Robot Arm) 机 器 人 是 日 本 山梨 大 学 牧野 洋 教 
授 发 明 的 一 种 用 于 装配 作业 的 机 器 人 手臂 ,有 三 个 转动 关节 和 一 个 移动 关节 ,如 图 2. 19(a) 
所 示 ,三 个 转动 关节 实现 平面 内 的 快速 定位 ,移动 关节 用 于 提取 和 放置 物品 。 


s s x 
(am a a 
(a) SCARA 机 器 人 (b) 机 构 简 图 


图 2.19 SCARA 机 器 人 及 其 机 构 简 图 
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图 2.19(a) 中 的 SCARA 机 器 人 的 机 构 简 图 如 图 2. 19(b) 所 示 。 从 该 机 构 简 图 中 可 以 
清楚 地 看 出 SCARA 机 器 人 三 个 转动 关节 是 轴线 相互 平行 的 关系 ,这 种 机 构 特 性 是 机 器 人 
照片 无 法 给 出 的 ,这 也 体现 了 机 构 简 图 的 独特 作用 。SCARA 机 器 人 末端 的 位 置 可 表示 为 
P 二 F(0,$,Y,7), 即 关节 变量 包括 3 个 转动 变量 和 1 个 移动 变量 。 

SCARA 机 器 人 工作 空间 的 形状 如 图 2. 20 
所 示 。 从 图 中 可 以 看 出 ,该 类 机 器 人 在 水 平面 和 
垂直 平面 内 都 具有 较 大 的 工作 空间 。 

SCARA 机 器 人 的 特点 包括 : 

(1) 在 水 平方 向 上 具有 和 柔顺 性 ,在 垂直 方向 
上 具有 和 良好 刚度 ,特别 适合 平面 定位 、 垂 直方 向 
装配 作业 ; 

(2) 运动 灵活 ,速度 快 ,速度 可 达 10m/s; 

(3) 采用 串联 的 两 杆 结构 ,类 似 人 的 手臂 ,可 图 2.20 SCARA 机 器 人 的 工作 空间 
伸 进 受 限 空间 中 作业 ; 

(4) 三 个 转动 关节 相互 平行 ,具有 耦合 性 ,运动 学 建 模 与 控制 比较 复杂 。 

5) 通用 多 关节 型 机 器 人 

通用 多 关节 型 机 器 人 主要 是 指 6 自由 度 或 5 自由 度 的 关节 型 工业 机 器 人 ,该 类 机 器 人 
一 般 是 由 五 个 或 六 个 独立 的 转动 关节 构成 ,主要 包括 基 座 .大 臂 和 小 臂 三 个 部 分 ,在 外 形 结 
构 上 类 似 人 的 有 骆 膊 ,如 图 2.21(a) 所 示 。 通 用 多 关节 型 机 器 人 可 采用 坐 立 式 . 吊 挂 式 和 和 斜 挂 
式 三 种 安装 方式 ,主要 用 于 焊接 、 装 配 搬运、 喷涂 等 ,具有 广泛 的 应 用 。 

对 于 图 2. 21(a) 中 的 6 自由 度 关节 型 机 器 人 ,其 机 构 简 图 的 画 法 有 两 种 ,如 图 2. 21(b) 
和 图 2. 21(c) 所 示 ,它们 的 主要 区 别 在 于 轴线 平行 于 纸 面 的 转动 关节 的 画 法 不 同 , 采 用 第 二 
种 画 法 更 简单 些 ,机 器 人 的 构 型 也 显得 更 简洁 ,所 以 目前 多 采用 第 二 种 画 法 来 表示 6 自由 度 
通用 多 关节 型 机 器 人 的 机 构 。 


(a) 6 自由 度 关节 型 机 器 人 (b) 机 构 简 图 1 (c) 机 构 简 图 2 
图 2.21 通用 多 关节 型 机 器 人 


通用 多 关节 型 机 器 人 工作 空间 的 形状 如 图 2. 22 所 示 。 从 图 中 可 以 看 出 ,该 类 机 器 人 在 
水 平面 和 竖 直 面 内 都 具有 较 大 的 工作 空间 ,与 前 面 介 绍 的 其 他 机 器 人 相 比 ,在 垂直 平面 内 具 
有 最 大 的 工作 空间 。 

通用 多 关节 型 机 器 人 具有 下 列 特点 : 

(1) 动作 灵活 ,工作 空间 大 ; 
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| 最 大 工作 半径 
I 


RB cN HB HR 


正视 图 


图 2.22 通用 多 关节 型 机 器 人 的 工作 空间 


(2) 结构 易于 实现 标准 化 ,具有 通用 性 ; 

(3) 速度 快 ,重复 定位 精度 高 ; 

(4) 绝对 定位 精度 相对 较 低 ,系统 刚度 低 ; 

(5) 各 关节 运动 具有 复杂 的 耦合 关系 ,运动 学 建 模 和 逆 解 复杂 ,控制 复杂 。 
上 述 五 种 串联 机 器 人 的 特点 对 比如 表 2. 2 所 示 。 


表 2.2 五 种 串联 机 器 人 的 特点 对 比 


类 型 运动 特点 工作 范围 | 结构 “| 定位 精度 | 所 占 空 间 
F IRAH X.Y.Z 三 个 坐标 | 
EAERI 轴 方 向 的 直线 运动 关节 组 成 N" K i 
F a BL38 A W N B. ll 3⁄2 _ fh #Ë , Ji 简单 
圆柱 坐标 机 器 人 apr Bx | aa | BE | æ 
机 器 人 辟 部 由 一 个 直线 运动 关 
节 与 两 个 回转 关节 组 成 , 即 一 个 
人 | ` 
i TET e Q Q| S | SM | 本 
组 成 
在 水 平方 向 上 具有 顺从 性 ,而 在 
SCARA 机 器 人 重 直 方向 具有 良好 的 刚度 , 速 | gh | 简单 | 很 高 | 较 小 
Eh 
个 组 成 ， 个 
通用 多 关节 型 机 器 人 人 | gk | aa 高 较 大 


2.4.2 并 联机 器 人 机 构 介绍 


并 联机 器 人 主要 包括 三 种 不 同 的 机 构 类 型 : Stewart 并 联机 器 人 、Delta 并 联机 器 人 、 
Tricept 并 联机 器 人 。 下 面 对 各 种 并 联机 器 人 的 机 构 、 特 点 和 用 途 进行 介绍 并 给 出 其 机 构 
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1) Stewart 并 联机 器 人 

1965 年 ,德国 人 Stewart 发 明了 6 自由 度 并 联机 构 , 用 于 设计 飞行 模拟 器 ,其 结构 如 
图 2. 23 所 示 ,采用 该 机 构 的 机 器 人 被 称 为 Stewart 并 联机 器 人 。Stewart 并 联机 器 人 主要 
由 动 . 静 两 个 平台 和 连接 两 个 平台 的 6 条 驱动 支 链 构成 ,通常 采用 液压 或 电 驱 动 , 主 要 特点 
包括 : 

(1) 具有 6 个 自由 度 ; 

(2) 运动 无 奇异 ,定位 精度 高 ,刚度 大 ,负载 大 ; 

(3) 运动 学 模型 有 极 强 的 非 线 性 ,运动 空间 小 ; 

(4) 单 根 支 链 上 的 定位 误差 被 其 他 支 链 平均 ,没有 累积 误差 ; 

(5) 球 关 节 是 被 动 的 ,没有 驱动 和 刹车 ,其 位 置 由 其 他 支 链 约束 确定 。 

图 2. 23(a) 中 的 Stewart 并 联机 器 人 的 机 构 简 图 如 图 2. 23(b) 所 示 。 该 机 器 人 主要 由 
球 副 和 移动 副 组 成 ,每 条 驱动 支 链 的 结构 比较 简单 ,但 因为 有 6 条 支 链 , 所 以 总 体 看 起 来 机 
构 简 图 比较 复杂 。 


(a) Stewart 并 联机 器 人 [1 (b) 机 构 简 图 
图 2.23 Stewart 并 联机 器 人 及 机 构 简 图 


Stewart 并 联机 器 人 主要 用 于 飞行 模拟 器 .粒子 加 速 器 中 的 电镜 或 磁 镜 机 构 ,高 精度 和 
大 负载 定位 器 等 ,如 图 2.24 所 示 。 


(a) 飞行 模拟 器 [9 ` (b) 电镜 机 构思 (c) 大 负载 定位 器 1 
图 2.24 Stewart 并 联机 器 人 的 应 用 
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关于 Stewart 机 构 的 名 称 是 有 一 


段 历史 冤案 的 。 英 国 的 Eric Gough 在 1954 年 就 研制 


了 一 种 基于 并 联机 构 的 6 自由 度 轮胎 检测 装置 ,如 图 2. 25 所 示 , 其 核心 机 构 就 是 我 们 介 


图 2.25 Gough 发 明 的 轮胎 检测 装置 中 


分 拣 动 作 。 


绍 的 Stewart 机 构 。1965 年 德国 人 Stewart 对 
Gough 发 明 的 这 种 机 构 进 行 了 机 构 学 意义 上 的 研 
ROT ,并 将 其 推广 应 用 于 飞行 模拟 器 ,因为 Stewart 
对 该 机 构 的 大 力 研 究 和 推广 ,所 以 人 们 就 把 这 种 机 
构 称 为 Stewart 机 构 。 但 实际 上 ,这 种 6 自由 度 并 联 
机 构 的 正确 名 称 应 为 Gough-Stewart 机 构 。 

2) Delta 并 联机 器 人 

Delta 并 联机 器 人 是 由 瑞士 洛桑 联邦 理工 学 院 
(EPFL) 的 Reymond Clavell” 教授 在 20 世纪 80 年 
代 初 提出 。Delta 并 联机 器 人 的 动 、 静 平台 之 间 有 三 
条 带 有 四 边 形 机 构 的 传动 支 链 ,三 个 驱动 器 安装 在 
静 平 台 上 ,未 端的 动 平台 只 有 X.Y.Z 三 个 方向 的 移 
动 ,没有 转动 自由 度 , 如 图 2. 26 所 示 。Delta 并 联机 
右 人 的 速度 非常 快 ,最 快 可 实现 300 次 /min 的 物品 


Delta 并 联机 器 人 的 特点 主要 包括 : 

d) 具有 3 个 自由 度 ( 也 有 4、5、6 自由 度 的 混 联 结构 ); 

(2) 静 平 台 安 装 3 个 驱动 器 ,有 3 条 带 四 边 形 机 构 的 驱动 支 链 , 通 常 采 用 电机 驱动 ; 

(3) 运动 无 奇异 ,定位 精度 高 ,负载 小 ; 

(4) 运动 学 模型 有 极 强 的 非 线 性 ,运动 空间 小 ; 

(5) 单 根 支 链 上 的 定位 误差 被 其 他 支 链 平均 ,没有 累积 误差 ; 

(6) 球 关节 是 被 动 的 ,没有 驱动 和 刹车 ,其 位 置 由 其 他 支 链 约束 确定 。 

图 2.26(a) 中 的 3 H HBE Delta 并 联机 器 人 的 机 构 简 图 如 图 2. 26(b) 所 示 , 主 要 由 球 副 
和 转动 副 构 成 ,三 条 支 链 具有 相同 的 结构 。 


(a) Delta 并 联机 器 人 (b) 机 构 简 图 
图 2. 26 Delta 并 联机 器 人 及 机 构 简 图 
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Delta 并 联机 器 人 主要 用 于 包装 /医药 行业 取 放 物品 .电子 产品 的 装配 、 触 觉 交互 设备 、 
3D 打印 设备 等 ,如 图 2. 27 所 示 。 


(a) 取 放 物品 的 Delta 机 器 人 9 (b) 3D 打 印 的 Delta 机 器 人 BO 
图 2.27 Delta 机 器 人 的 应 用 


由 于 传统 的 Delta 并 联机 器 人 只 有 3 个 平 动 自由 度 , 在 一 些 应 用 场合 需要 机 器 人 具有 
转动 自由 度 , 即 调整 姿态 的 能 力 ,因此 具有 4 个 或 更 多 自由 度 的 Delta 并 联机 器 人 逐步 被 开 
发 出 来 。 如 图 2. 28 所 示 是 ABB 公司 的 4 自由 度 Delta 并 联机 器 人 ,该 机 器 人 是 在 传统 3 自 
由 度 Delta 并 联机 器 人 的 基础 上 增加 了 1 个 自由 度 的 旋转 运动 ,该 自由 度 相 对 于 其 他 3 个 
并 联 支 链 是 独立 的 ,严格 意义 上 讲 这 是 一 个 串 .并 联 混 合 机 器 人 。 


(a) 4 自由 度 Delta 机 器 人 [3 (b) 机 构 简 图 
图 2.28 4 自由 度 的 Delta 机 器 人 及 其 机 构 简 图 


3) Tricept 并 联机 器 人 

1985 年 ,瑞典 Neos Robotics 公司 创始 人 和 总 裁 纽 曼 (Karl-Erik Neumann) 发 明了 
Tricept 并 联机 器 人 ,如 图 2. 29 所 示 。 这 是 一 个 5 自由 度 的 串 、 并 联 混合 机 构 : 并 联 部 分 有 
3 RLH H 3 个 自由 度 ,驱动 安装 在 静 平 台 上 ; 2 自由 度 的 串联 机 构 安 装 在 动 平 台 上 。 因 
为 此 发 明 ,纽曼 先生 在 1999 年 被 授予 国际 金 机 器 人 奖 (Golden Robot Award) ,这 是 工业 领 
域 最 有 威信 的 奖项 之 一 。 

Tricept 机 器 人 具有 下 列 特点 : 

(1) 5 个 自由 度 , 串 并联 混合 结构 ,并 联 3 自由 度 , 串 联 2 自由 度 ; 

(2) 静 平 台 安装 有 3 条 支 链 , 动 平台 未 端 连 接 2 自由 度 串 联结 构 ; 

(3) 定位 精度 高 ,负载 大 ; 
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(4) 工作 空间 大 ,刚度 高 。 


图 2.29 纽曼 与 Tricept 并 联机 床 “ 


Tricept 机 各 人 主要 由 移动 副 .转动 副 等 构成 ,如 图 2. 30(a) 所 示 , 其 机 构 简 图 如 图 2. 30(b) 


(a) Tricept 并 联机 器 人 (b) 机 构 简 图 
图 2. 30 Tricept 并 联机 构 及 其 机 构 简 图 


目前 ,Tricept 并 联机 器 人 主要 用 于 汽车 装配 自动 生产 线 ,完成 加 工 、 装 配 、 焊 接 等 工序 ， 
如 图 2.31 所 示 。 


图 2.31 Tricept 并 联机 器 人 应 用 


Stewart 并 联机 器 人 
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上 面 介绍 的 3 种 并 联机 器 人 的 特点 对 比如 表 2. 3 所 示 。 
表 2.3 3 种 并 联机 器 人 的 特点 对 比 


名 K 自由 度数 | 工作 范围 


Delta 并 联机 器 人 


Tricept 并 联机 器 人 


2.5 小结 


本 章 主要 介绍 了 机 器 人 机 构 的 类 型 及 其 机 构 简 图 的 画 法 ,重点 介绍 了 5 种 串联 机 器 人 


机 构 和 3 种 并 联机 器 人 机 构 的 结构 、 特 点 ,用 途 及 机 构 简 图 。 掌 握 机 器 人 机 构 简 图 的 绘制 方 
法 是 设计 和 分 析 机 器 人 机 构 的 基础 。 需 要 说 明 的 是 ,机 器 人 的 机 构 简 图 是 一 种 象形 画 法 , 目 
前 还 没有 统一 的 绘制 方法 。 
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CHAPTER 3 


在 机 器 人 学 中 ,机 器 人 的 位 置 (Position) 和 姿态 (Posture / Pose) 常 常 被 统称 为 位 姿 。 
本 章 主 要 介绍 机 器 人 位 姿 的 数学 描述 方法 以 及 不 同 坐 标 系 之 间 的 坐标 变换 方法 。 其 中 ,位 
姿 的 数学 描述 是 表达 机 器 人 的 线 速度 、 角 速度 、 力 和 力矩 的 基础 ,而 坐标 变换 是 研究 不 同 坐 
标 系 中 的 机 器 人 位 姿 关 系 的 重要 途径 。 


3.1 引言 


机 器 人 通常 由 一 系列 构件 和 运动 副 组 合 而 成 , 它 
能 够 在 三 维 空间 中 实现 各 种 复杂 运动 和 预定 操作 。 为 
了 实现 机 器 人 的 运动 及 操作 ,就 需要 表达 出 操作 对 象 、 
工具 以 及 机 器 人 本 体 的 位 置 与 姿态 。 以 图 3. 1 中 的 机 
器 人 为 例 , 为 了 让 机 器 人 抓 取 传 送 带 上 的 包 庄 ,首先 需 
要 检测 出 包 庄 的 位 置 和 姿态 ,然后 控制 机 器 人 末端 机 
械 手 的 位 置 和 姿态 到 达 预 定 值 。 很 显然 ,位 姿 是 机 器 
人 控制 和 应 用 中 非常 重要 的 变量 。 为 了 能 使 用 表示 机 图 3.1 机 器 人 抓 取 包 囊 吕 
器 人 位 姿 的 变量 ,首先 必须 给 出 具有 通用 性 的 定义 和 
表达 规则 。 


3.2 机 器 人 位 姿 的 数学 描述 


位 姿 的 概念 与 自由 度 的 概念 是 紧密 联系 在 一 起 的 。 在 运动 学 中 ,描述 三 维 空间 中 物体 
的 运动 需要 6 个 变量 (zx,y,z,a,B,7) ,因此 ,一 个 在 三 维 空间 中 自由 运动 的 物体 具有 6 个 自 
由 度 , 这 6 个 变量 中 ,(x,y,z)3 个 变量 表示 位 置 ,(a,B,7)3 个 变量 表示 姿态 。 

为 了 描述 机 器 人 的 位 置 和 姿态 ,需要 建立 坐标 系 。 在 机 器 人 学 中 ,通常 采用 笛 卡 儿 坐 标 
系 (Cartesian Frame) ,就 是 常用 的 三 轴 正 交 坐 标 系 。 

笛 卡 儿 (CReng Descartes,1596 一 1650), 见 图 3. 2 ,是 法 国 著名 的 哲学 家 、 数 学 家 、 物 理学 
家 ,他 创立 了 著名 的 平面 直角 坐标 系 ,使 得 几何 形状 可 以 用 代数 公式 明确 表达 出 来 , 因 将 笛 
卡 儿 坐标 体系 公式 化 而 被 称 为 解析 几何 之 父 , 常 用 的 直角 坐标 系 也 被 称 为 笛 卡 儿 坐 标 系 。 
在 笛 卡 儿 所 处 的 时 代 ,拉丁 文 是 通用 的 学 术语 言 , 笛 卡 儿 通常 会 在 他 的 著作 上 签 上 他 的 拉丁 
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名 字 Renatus Cartesius, 而 Cartesian 是 Cartesius 的 形容 词 
形式 ,所 以 他 创立 的 直角 坐标 系 被 称 纹 Cartesian Frame, $É 
卡 儿 的 姓 Descartes( 法 语 ) 是 由 des 和 Cartes 组 成 的 复合 词 ， 
对 应 的 英文 是 “of the Maps”, 这 说 明 笛 卡 儿 的 祖先 可 能 是 从 
事 地 图 绘制 的 。 
由 于 通常 认为 机 器 人 的 连 杆 和 关节 都 是 刚性 的 ,因此 
在 后 续 的 内 容 中 ,讨论 的 对 象 都 假设 为 刚性 物体 , 简称 
刚体 。 
如 图 3.3 所 示 , 这 里 有 一 个 刚体 ,为 了 描述 该 刚体 的 位 置 
图 3.2 ERLA 和 姿态 总 共 需 要 建立 两 个 坐标 系 , 一 个 是 参考 系 {i 让, 它 相 对 
于 被 描述 刚体 是 静止 不 动 的 , 男 一 个 是 建立 在 被 描述 刚体 上 的 坐标 系 (7}) ,被 称 为 刚体 坐标 
系 。 需 要 说 明 的 是 ,这 两 个 坐标 系 的 建立 是 任意 的 ,也 就 是 说 可 以 建立 的 参考 系 和 刚体 坐标 


系 的 组 合 是 无 穷 多 的 。 
图 3.3 位 姿 的 数学 描述 
在 图 3. 3 中 ,假设 刚体 坐标 系 {7} 的 原点 O, 在 参考 系 { 引 中 的 坐标 为 (zo + yo + zo) + JI) iZ 
刚体 的 位 置 可 表示 为 : 
To 
二 > Ea) 
Zo 
即 刚体 的 位 置 用 一 个 3X1 的 列 矢 量 表示 。 
刚体 的 姿态 则 用 一 个 3X3 的 矩阵 表示 为 : 
m= [X _ iY Z], 
cos( Z X,X;) cos( ZY; X,) cos(Z Z;X,) 
= |cos(Z X, ,Y,) cos(ZY;Y;) “cos(Z Z;Y;) (3-2) 
cos( Z X,Z;) cos(ZY;Z,) cos(C ZZ.) 


RG-DP X Y Z 为 三 个 单位 正 交 主 矢量 ,分 别 表示 刚体 坐标 系 {7) 的 三 个 坐标 轴 
X,.Y,.Z, EBZ RD PRH, ZX; X: 表示 坐标 轴 X 与 坐标 轴 X , 之 间 的 夹 角 , 其 他 的 
夹 角 表示 含义 与 此 类 似 。 

在 刚体 的 位 置 矢 量 ;P 和 姿态 和 矩阵 R 中 ,左上 标 表 示 被 描述 的 刚体 坐标 系 , 左 下 标 表示 
参考 系 , 如 左上 标 j; 表示 刚体 坐标 系 {j) ,左下 标 i 表 示 参 考 坐 标 系 { 让 ,本 书 的 后 续 内 容 中 也 
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将 沿用 此 上 下 标的 规定 。 当 位 置 撩 量 和 姿态 矩阵 的 左上 下 标 发 生变 化 时 ,如 左上 标 变 为 i， 
左下 标 变 为 j, 则 意味 着 刚体 坐标 系 和 参考 系 也 发 生 了 变化 。 
BREER 具有 下 列 特点 。 
(1) iR 共有 9 个 元 素 , 只 有 3 个 是 独立 的 ,有 6 PARRI: 
IX JK =i Jy = 2 Sa 
IX +Y =Y Z =Z +X = 0 
(2) ¿R 是 单位 正 交 阵 ,具有 下 列 特 点 : 
iR” =R', |iR|=1 
姿态 矩阵;R 是 正 交 阵 的 特点 可 极 大 简化 其 求 着。 众所周知 ,矩阵 求 逆 一 般 比 较 复 杂 ， 
IB H T/R 矩阵 的 道 等 于 其 转 置 ,所 以 可 以 很 容易 地 求 得 其 逆 矩 阵 。 
例 1: 姿态 矩阵;R 的 含义 是 什么 ? 它 与 姿态 矩阵 ;R 有 什么 关系 ? 
解 : 
参考 图 3.3, BEER 表示 的 是 刚体 坐标 系 人 i} 相对 于 参考 系 {7 的 姿态 。 
[cos(ZX:X;) cos(ZY,X;) cos( Z Z,X;) 
iR = [}X_}Y iZ] = |[cos( Z X,Y;) cos(ZY,Y;) cos(Z Z;Y;) | =;RT =R” 
OZ ML) “cos( ZY; Z;) wo LL 
在 几何 关系 上 ,姿态 矩阵 ;R 是 姿态 和 矩阵;R 的 道 。 在 数值 关系 上 ,姿态 矩阵 ;R EBEE 
HEIR 的 转 置 。 
例 2: 某 刚 体 如 图 3. 4 所 示 , 建 立 刚体 坐标 系 {j} 和 参考 
系 { 引 , 求 该 刚体 的 位 置 矢量;'P 和 姿态 矩阵 'R。 
解 : 
参考 图 3. 4 中 两 坐标 系 的 位 姿 关 系 及 刚体 位 置 和 姿态 的 
定义 , 求 得 刚体 的 位 置 矢量 和 姿态 矩阵 分 别 为 : 


一 10 1 0 0 
iP= 0l, iR=|0 1 0 
6 0 gi 


这 里 求 得 的 姿态 矩阵 ;R 是 一 个 单位 阵 , 而 在 图 3.4 H, 
标 系 { 引 和 {7} 的 对 应 坐标 轴 方向 是 相同 的 ,这 说 明 由 姿态 和 矩 
阵 可 唯一 确定 刚体 坐标 系 相 对 于 参考 系 的 方位 。 


图 3.4 刚体 的 位 姿 


3.3 点 的 坐标 变换 

点 的 坐标 变换 主要 包括 坐标 系 的 平移 .坐标 系 的 旋转 以 及 包含 平移 与 旋转 的 综合 变换 ， 
下 面 分 别 对 这 三 种 变换 进行 介绍 。 

3.3.1 坐标 系 平移 (姿态 相同 ) 


如 图 3. 5(a) 所 示 ,假设 点 了 在 坐标 系 {7} 中 的 位 置 卫 已 知 ,如 果 将 坐标 系 {7) 平 移 至 坐 
标 系 (让 处 ,如 何 求 点 P 卫 在 坐标 系 { 习 中 的 位 置 'P? 
该 问题 可 采用 矢量 三 角形 来 求解 。 构 造 图 3. 5(b) 中 所 示 的 矢量 三 角形 0;0;P, 则 可 得 
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(a) (b) 


图 3.5 坐标 系 平移 


REFN: 
OP = 00, +OP 
由 于 矢量 OP .OP 分 别 在 坐标 系 { 引 、(7} 的 三 个 坐标 轴 上 的 投影 就 是 点 了 在 坐标 系 {2) 
和 {站 中 的 位 置 坐 标 'P 和 PP, 所 以 上 述 和 失 量 等 式 可 表示 成 以 下 形式 : 
iP =P + P (3-3) 
式 (3- SME T A P ERRAN } 中 的 位 置 。 从 该 公式 可 以 看 出 ,点 P 在 坐标 系 {i Si 
位 置 除了 与 该 点 在 坐标 系 人 7} 中 的 位 置 相 关外 ,还 与 坐标 系 { 站 的 坐标 原点 O, 在 坐标 系 {i 


中 的 位 置 %P 相关 。%P 是 由 坐标 系 { 站 平移 到 举 标 系 { 让 产生 的 ,WP 一 [%z y =]. 


Nn ee A MT ER 
量 , 因 此 在 不 同 坐 标 轴 上 的 位 移 可 以 累加 ,而 且 不 受 移动 顺序 的 限制 , 即 : 
E Dar YY Ax 
0 9 >; Ay 
0 6, 2; Az DD) Az 

在 上 式 中 ,Az、Ay、Az miu Tasu RE EE Bt , H J P 表示 的 是 坐标 
£ (j) BJ A Pp JBL 5 #E $e PR Z (G) PEILE, Z 3 # 2 E TEM, Br 1 SE SE H 4 (9 8 A 
取 负 。 

在 图 3.5 中 ,坐标 系 (7) 与 坐标 系 { 沾 之 间 的 关系 既 可 描述 成 平移 关系 ,也 可 以 描述 成 平 
行 关系 , 即 如 果 两 个 坐标 系 相 互 平行 ,知道 一 个 点 在 一 个 坐标 系 中 的 位 置 , 则 该 点 在 男 一 
坐标 系 中 的 位 置 也 可 以 用 式 (3-3) 求 得 。 

式 (3-3) 适 用 于 图 3. 6 所 示 的 直角 坐标 机 器 人 的 运动 学 计算 。 该 机 器 人 的 三 个 关节 分 
别 在 参考 系 {0} 的 Xi Yo Zo 三 个 坐标 轴 方 向 上 移动 ,这 些 移动 量 改变 的 是 机 器 人 未 端 坐标 
系 {e} 的 原点 O, 相对 于 参考 坐标 系 10} 的 位 置 ~P。 由 于 直角 坐标 机 右 人 的 关节 运动 变量 是 
移动 量 , 所 以 该 类 机 器 人 只 能 改变 机 器 人 末端 工具 的 位 置 , 不 能 改变 其 姿态 。 

例 3: 如 图 3.7 所 示 , 坐 标 系 {7}) 由 坐标 系 {让 沿 着 Y 轴 方 向 平移 而 得 ,已 知 点 了 在 坐标 


0 


— DAy 


0 


0 


十 十 
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图 3.6 直角 坐标 机 器 人 


系 {让} 中 的 位 置 为 全 二 [一 5 6 7]7, 求 点 也 在 坐标 
系 { 让 中 的 位 置 。 
解 : 
由 式 (3-3) 可 得 点 卫 在 坐标 系 { 引 中 的 位 置 为 : 
站 =%YP 十 站 
= [Q 15 0 十 [= 6 7]J' 
=[—5 21 7J 
jk Hi, ARA (2) R| BH A h AE bA UNEA Y, HH 
方向 平移 一 15 而 得 , 即 Ay 二 一 15。 


3.3.2 ”上 坐标 系 旋 转 ( 原 点 相同 ) 


问题 : 如 图 3. 8 所 示 ,坐标 系 { 刀 由 坐标 系 { 守 旋转 而 成 ,两 坐标 系 具有 相同 的 坐标 原点 。 
已 知 点 P 在 坐标 系 {j} 中 的 位 置 为 P 卫 二 [zx y; g] RA P 3 PE # U) rh 89 w # 
P= y as 


图 3.7 坐标 系 平移 


图 3.8 坐标 系 旋转 图 3.9 坐标 变换 投影 图 
解 : 
该 问题 可 采用 如 图 3.9 所 示 的 矢量 投影 的 方法 来 求解 。 具 体 的 解 题 步骤 是 : Qa P fE 


50 l| 机 器 人 学 一 机 构 、 运 动 学 、 动 力学 及 运动 规划 


标 系 {j} 中 的 位 置 变量 xz; .y 、z; 就 是 矢量 OP 在 坐标 轴 Xi Y Z 上 的 投影 ,将 三 个 位 置 变 
量 分 别 与 各 自 所 在 的 坐标 轴 组 合 构 成 三 个 矢量 x; 、y; ,zj ,再 将 三 个 矢量 分 别 向 坐标 系 { 引 的 
三 个 坐标 轴 X: Y: Z: 投影 ,在 每 个 坐标 轴 上 投影 的 标量 和 就 是 点 卫 在 坐标 系 { 引 中 的 位 置 。 
下 面 以 求 y: 为 例 说 明 。 

首先 ,把 矢量 x; 向 Y, 轴 投 影 , 得 y, 分 量 为 : 

y, = z;cos(ZY,X;) 
再 把 矢量 y; 向 Y; 轴 投 影 ,得 w 的 分 量 和 为 : 
Yi = zw;cos(ZY,X;) + y;cos( ZY;Y;) 
最 后 ,把 矢量 z) 向 Y, 轴 投 影 ,得 y, 的 分 量 和 为 : 
y; = z;cos(ZY,X ;) + y;cos( ZY,Y;) + z;cos( ZY;Z;) 

这 就 是 点 P EBRR Y: 轴 上 的 位 置 坐标 值 。 

利用 此 方法 可 分 别 求 得 点 PP 在 X;、Z; 两 轴 上 的 位 置 坐标 值 zx、 六 ,由 此 得 到 点 P ZEA 
标 系 {z} 中 的 位 置 为 : 

=; = z;cos( Z X,X;) + y;cos( Z X,Y,) + z;cos( í X,Z;) 
'P = ° = z;cos( ZY,X;) + y;cos(ZY,;Y;) + z;cos( ZY,Z;) 
z; = z;cos( Z Z,X ,) + y;cos(( Z,Y;) + z;cos( (í Z,Z;) 

将 上 式 写 成 矩阵 形式 ,可 得 : 
cos(ZX,X;) cos(ZX:Y;) cos( í X,Z;) 
sL ZYX; ) “cos(2ZYY;) cost Y. Z;) 
bos( Z ZX) cos( ZZY; cosl Zi) 

由 于 该 等 式 右 边 的 第 一 个 矩阵 与 前 面 介 绍 的 姿态 矩阵 [ 式 (3-2)] 相 同 , 所 以 该 矩阵 可 
以 表示 为 : 


Tj 


iP = 


Yj 


Zj 


cos(ŻX:X;) cos(ZX:Y,) cos(ZX X,Z,) 
ceosC ZY KX, ) cost ÆY:Y;) cosCZVY,Z;) 
cos(ZZ,X;) cos(ZZ,Y,) cos(Z Z,Z;) 

所 以 点 卫 在 坐标 系 {z} 中 位 置 的 计算 公式 可 简写 为 : 

'P =;R:P (3-4) 

其 中 ,IR 表示 坐标 系 {j} 相 对 于 坐标 系 {让 的 姿态 ,是 3X3 的 矩阵 ,这 里 该 矩阵 被 称 作 旋 

转变 换 矩 阵 ,简称 旋转 矩阵 。 旋 转 和 矩阵 具有 与 姿态 矩阵 相同 的 特性 : 
iR™ =R" =;R 

上 面 介绍 的 是 坐标 系 旋转 的 一 般 情 况 , 式 (3-4) 对 于 共 原 点 的 两 坐标 系 之 间 的 任意 旋 
转 都 适用 。 那 么 如 何 计算 旋转 矩阵 呢 ? 下 面 分 类 介绍 。 

1. 坐标 系 绕 单个 坐标 轴 旋 转 的 旋转 矩阵 

坐标 系 绕 单个 坐标 轴 旋 转 的 问题 可 描述 为 : 一 个 坐标 系 { 引 绕 其 某 个 坐标 轴 旋 转 9 角 产 
生 新 坐标 系 ({j} , 求 旋转 矩阵 ;RR。 坐 标 系 绕 单 个 坐标 轴 旋转 共 包 括 三 种 情况 : 绕 X 轴 旋 转 0 
fa Ge Y 轴 旋 转 0 角 和 绕 2 轴 旋 转 0 角 , 如 图 3. 10 所 示 。 


R= 
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(a) HEXANE Of (b) se Yhlllettoft (c) 绕 z 轴 旋转 0 角 
图 3. 10 “坐标 系 绕 单个 坐标 轴 旋 转 9 角 
由 图 3. 10 中 的 坐标 系 {7 与 坐标 系 { 丰 之 间 的 几何 关系 ,根据 旋转 矩阵 的 定义 ,可 分 别 
得 到 绕 X 轴 、 了 轴 、Z 轴 转动 0 角 对 应 的 旋转 变换 矩阵 ,如 表 3. 1 所 示 。 


表 3.1 绕 单 个 坐标 轴 旋 转 0 角 的 旋转 矩阵 
旋转 描述 i 旋转 矩阵 


1 ü 0 

绕 X 轴 旋 转 9 fi iR=R(X,0)= |0 cos0 -am 
0 sing cos 
cos 0 sing 

绕 Y 轴 旋 转 0 角 | 0 i Q | 
—sing 0 cosô 
cosO —sin0 0 

绕 Z 轴 旋转 0 角 iR=R(Z,0)= 区 cos0 | 
0 0 1 


表 3. 1 中 的 三 个 旋转 矩阵 具有 下 列 特点 : 

(1) 旋转 矩阵 中 元 素 1 所 在 的 行 号 或 列 号 与 绕 哪 个 坐标 轴 转 动 相对 应 (1 二 X; 2=Y; 
3 >Z); 

(2) 主 对 角 线 上 都 有 一 个 元 素 为 1, 其 余 均 为 转角 的 余弦 或 正弦 ; 

(3) 在 元 素 1 所 在 的 行 和 列 ,其 他 元 素 均 为 0; 

(4) 从 元 素 1 所 在 的 行 起 ， 自 上 而 下 , 先 出 现 的 正弦 为 负 , 后 出 现 的 正弦 为 正 ; MZ 

绕 单 个 坐标 轴 旋 转 的 旋转 矩阵 的 上 述 特 点 可 以 帮助 我 们 更 容易 地 记 住 它们 。 

B|) 4: 坐标 系 { 让 绕 其 XX 轴 旋 转 45 ,产生 新 坐标 系 {7) ,假设 点 忆 在 坐标 系 {7) 中 的 位 置 
为 ;P==[10,0,20]" ,3 它 在 坐标 系 { 引 中 的 位 置 'P。 

解 : 

该 题 可 用 公式 (3-4) 和 绕 X 轴 旋 转 0 角 的 旋转 矩阵 通 式 ( 表 3. 1) 求 解 。 


10 10 
. 四 -aa 
20 


: 1 0 0 
iP =iR +P = R(X,0) *P -| cos45° — sin45° 
10./2 


0 sin45° cos45° 
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如 果 不 知 道 坐标 系 {7)} 如 何 由 坐标 系 { 人 2 旋转 生成 的 ,但 两 坐标 系 之 间 位 姿 关 系 符 合 坐 
标 系 绕 单 轴 旋 转 0 角 的 情况 , 则 同样 可 以 用 式 (3-4) 和 表 3.1 中 的 旋转 和 矩阵 通 式 进 行 坐标 变 
换 的 计算 。 

2. 坐标 系 绕 多 个 坐标 轴 转 动 的 旋转 矩阵 

坐标 系 绕 多 个 坐标 轴 转 动 问题 可 分 为 绕 动 坐标 系 的 多 个 坐标 轴 旋 转 和 绕 定 坐 标 系 的 多 
个 坐标 轴 旋 转 两 类 问题 。 下 面 介 绍 这 两 类 转动 问题 的 旋转 矩阵 计算 方法 。 

1) 绕 动 坐标 系 的 多 个 坐标 轴 旋 转 的 旋转 矩阵 

问题 : 如 图 3.11 所 示 ,坐标 系 {z 绕 其 Z 轴 旋 转 y 角 , 得 到 新 坐标 系 {1} ,坐标 系 (1}) 再 
绕 其 立轴 旋转 0 角 , 得 到 新 坐标 系 {2) ,坐标 系 12} 再 绕 其 Z 轴 旋 转 $ 角 , 得 到 新 坐标 系 {7)， 
求 旋转 矩阵 RC(g,0,$)。 

解 : 

在 坐标 系 { 让 旋转 到 坐标 系 {7} 的 过 程 中 ,三 个 坐标 
系 的 三 个 坐标 轴 先 后 被 作为 旋转 参考 轴 。 

下 面 来 推导 如 何 求 得 旋转 矩阵 RC(g,0,$)。 

假设 有 一 点 卫 , 它 在 坐标 系 47)} 中 的 位 置 为 也 , 求 它 
在 坐标 系 { 沾 中 的 位 置 卫 。 

首先 ,以 坐标 系 47} 和 坐标 系 {2} 为 对 象 ,这 两 个 坐标 
系 之 间 的 转换 关系 属于 绕 2 轴 旋 转 一 个 角度 ,采用 


式 (3-4) 计 算 点 卫 在 坐标 系 {2} 中 的 位 置 : 图 3.11 绕 动 坐标 系 的 坐标 轴 


然后 ,以 坐标 系 {2} 和 坐标 系 {1} 为 对 象 ,这 两 个 坐 
标 系 之 间 的 转换 关系 属于 绕 Y 轴 旋 转 一 个 角度 0, 采用 式 (3-4) 计 算 点 也 在 坐标 系 {(1} 中 的 
位 置 : 
'P 一 ?R .P= R(Y,0°P 
最 后 ,以 坐标 系 {1} 和 坐标 系 {2 为 对 象 , 这 两 个 坐标 系 之 间 的 转换 关系 属于 绕 Z 轴 旋 转 
一 个 角度 g, 采 用 式 (3-4) 计 算 点 卫 在 坐标 系 { 引 中 的 位 置 : 
iP =!R "P= R(Z,ọ)'P 
KARIER P P 依次 代入 上 式 , 可 得 : 
P-R l P= RZ aP 
= R(Z,e)R(Y 0° P 


= R(Z,e)R(Y.0)R(Z,$)! P 
=İR iP 
所 以 可 得 : 
‘R=R(Z,9)R(Y ,0R(Z,$) 
即 


将 各 旋转 矩阵 的 表达 式 代 入 上 式 可 得 : 
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cosp 一 Sinp o| 0 sin] jcosy — sing 0 


iR(g,0,$)= “w cosp 0 


0 1 0 sing cos 0 
0 0 1] |= sin? @ cos 0 0 1 
| cosecos0cos$ — singsing — cosgcos0sinó 一 singcos$ cosgsin0 


singcos0cos$ + cosgsinó —singcosOsing cospcos$ singsing 


sinĝsin$ sin0sin$ cos0 

除 第 一 次 坐标 系 旋转 环绕 的 坐标 轴 外 ,后 面 两 次 旋转 环绕 的 坐标 轴 都 是 新 生成 坐标 系 
的 坐标 轴 , 这 些 新 生成 的 坐标 系 被 称 为 动 系 ,而 坐标 系 ( 让 被 称 为 定 系 。 所 以 ,上 述 坐 标 系 
{让 旋转 到 坐标 系 (站 } 的 过 程 被 称 为 绕 动 系 的 坐标 轴 旋 转 , 旋 转 矩 阵 R(g,0,$) 等 于 绕 三 个 坐 
标 轴 转动 的 旋转 矩阵 的 顺序 乘积 。 

2) 绕 定 坐标 系 的 多 个 坐标 轴 转 动 的 旋转 矩阵 

问题 : 如 图 3. 12 所 示 ,坐标 系 ( 沾 绕 其 X 轴 旋 转 a 角 , 得 到 新 坐标 系 {m} ,坐标 系 {m} 再 
绕 坐 标 系 (让 的 Z 轴 旋 转 9 角 , 得 到 新 坐标 系 {j} , 求 旋转 矩阵 :R (a,0)。 

解 : 

在 坐标 系 { 沾 旋转 到 坐标 系 {7} 的 过 程 中 ,坐标 系 { 引 的 两 个 
坐标 轴 先 后 被 作为 旋转 参考 轴 。 

推导 旋转 矩阵 及 (Ce,0) 的 过 程 分 两 步 : 

(1) 考虑 坐标 系 4z) 和 坐标 系 {z} ,假设 它们 初始 位 姿 重 合 ， 
坐标 系 {m}) 固 连 一 个 点 了 ,旋转 前 该 点 在 坐标 系 {i} 和 坐标 系 
{m}) 中 的 位 置 都 为 PP。 坐 标 系 {m} 绕 坐标 系 { 让 的 XX 轴 旋 转 a 
角 后 ,该 点 了 在 坐标 系 { 引 中 的 坐标 设 为 *P, 由 于 固 连 关系 ,点 图 3.12 绕 固定 坐标 轴 旋 转 
忆 在 坐标 系 {m} 中 的 位 置 值 仍 为 呈 , 则 存在 下 列 转换 关系 : 示意 图 

mP =7rR oP = R(X,a);P 

(2) 考虑 坐标 系 { 让 和 坐标 系 {7) ,它们 初始 位 姿 重 合 ,假设 坐标 系 人 7} 与 点 己 固 连 , 则 旋 
转 前 该 点 在 坐标 系 { 站 和 坐标 系 {7)} 中 的 位 置 坐标 为 ?PP。 坐 标 系 { 尹 绕 坐 标 系 {z 的 Z 轴 旋 转 
0 角 后 ,该 点 也 在 坐标 系 { 革 中 的 坐标 设 为 ,已 , 由 于 固 连 关 系 , 该 点 已 在 坐标 系 47} 中 的 坐标 
为 ”PP , 则 存在 下 列 转换 关系 : 

IP =iR “P = R (Z,0)”P = R(Z,0)R (X,a)!P 

中 为 旋转 前 点 了 ERR PME P 是 经 过 两 次 旋转 后 的 点 己 在 坐标 系 { 引 中 的 

位 置 ,所 以 坐标 系 {7} 与 坐标 系 { 寻 之 间 的 旋转 矩阵 为 : 
(有 Ru,0) = R(Z.0)R(X a) 


cos —sin0 0|1[1l 0 0 cos — sinĝcosa sinĝsina 
/Ra:,0) = |sing cosðô 0||O cosa — sina|= |sing cosgcosa 一 cosgsina 
0 0 1) 10 sma ` Gosa 0 sina cosa 


A E] APR (;) BJ 8 4- e bu h X Z 转动 的 旋转 矩阵 等 于 绕 Z 轴 和 绕 X 轴 转 动 
的 两 个 旋转 和 矩阵 的 乘积 。 

对 于 该 问题 ,本 章 参考 文献 还 给 出 了 另外 一 种 方法 , 供 参 考 。 

特别 提醒 : 多 个 旋转 矩阵 连 乘 时 ,次 序 不 同 则 含义 不 同 。 

绕 定 坐 标 系 和 绕 动 坐标 系 的 多 个 坐标 轴 转 动 时 ,其 最 终 坐 标 系 与 初始 坐标 系 之 间 的 旋 
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转 矩 阵 的 计算 规律 如 下 : 

(1) 绕 多 个 动 坐标 系 的 坐标 轴 依 次 转动 时 ， BAUNEI PIDI IRR NDES E 
等 于 每 个 绕 单 坐 标 轴 旋 转 的 旋转 矩阵 从 左 往 右 乘 的 乘积 , 即 旋转 矩阵 的 相 乘 顺 序 与 坐标 系 
转动 次 序 相同 ; 

(2) 绕 定 坐 标 系 的 多 个 坐标 轴 依 次 转动 时 ,最 终 坐标 系 
相对 于 初始 坐标 系 的 旋转 矩阵 等 于 每 个 绕 单 坐标 轴 旋 转 的 
旋转 矩阵 从 右 往 左 乘 的 乘积 , 即 旋转 矩阵 的 相 乘 顺序 与 坐标 
系 转动 次 序 相 反 。 

坐标 系 旋转 变换 矩阵 可 用 于 具有 旋转 关节 的 机 器 人 的 
运动 学 计算 ,如 图 3. 13 所 示 。 

通过 前 面 关于 坐标 变换 的 介绍 可 以 看 出 : 基于 坐标 系 旋 
转 的 坐标 变换 比 基 于 坐标 系 平移 的 坐标 变换 复杂 得 多 。 但 
现实 情况 是 , 现 有 的 机 器 人 中 旋转 关节 比 移动 关节 普遍 , 因 
图 3.13 ”旋转 关节 机 器 人 中 ”此 基于 坐标 系 旋转 的 坐标 变换 比 基 于 坐标 系 平移 的 坐标 变 

换 应 用 更 广泛 。 


3.3.3 坐标 变换 综合 (平移 十 旋转 ) 


问题 : 如 图 3.14 所 示 ,坐标 系 { 让 和 坐标 系 {7) 的 姿态 不 
相同 ,也 不 共 原 点 ,如果 已 知 点 了 在 坐标 系 {7} 中 的 位 置 , 如 何 求 出 其 在 坐标 系 ( 沾 中 的 位 置 ? 
解 : 
假设 一 个 中 间 华 标 系 {c} , 它 与 坐标 系 {)} 的 原点 重合 ,与 坐标 系 (让 的 姿态 相同 。 
由 于 坐标 系 {c} 与 坐标 系 {z} 的 姿态 相同 ,所 以 它们 之 间 是 纯 平 移 关 系 , 则 ; 


'R =R 
考虑 坐标 系 {7 ) ,两 个 坐标 系 之 间 是 纯 旋转 关系 ,假设 已 知 点 也 在 坐标 系 147} 中 的 
WEP, WA P Te 
P =iRiP —R:P 


由 于 坐标 系 {c} 与 坐标 系 {7} 原 点 重合 ,所 以 有 "PP 一 ，P。 
考虑 坐标 系 {让 和 {c) ,计算 点 了 在 坐标 系 (让 中 的 位 置 为 : 
ip =p HP = 人 PP 
由 此 可 求 得 两 个 姿态 不 同 、 原 点 不 同 的 坐标 系 之 间 的 点 位 置 转换 计算 公式 为 : 
iP =iRiP HP (3-5) 
此 公式 由 旋转 和 平移 两 部 分 组 成 。 
对 于 该 问题 ,还 有 另外 一 种 求解 方法 。 
如 图 3. 15 所 示 ,假设 一 个 中 间 坐 标 系 {c}, 它 与 坐标 系 {z} 原 点 重合 ,与 坐标 系 17) 姿态 
相同 。 
考虑 坐标 系 {j} 和 {c) ,它们 之 间 是 纯 平 移 关 系 ,假设 已 知 点 了 在 坐标 系 {7} 中 的 位 置 'P， 
则 点 卫 在 坐标 系 {c) 中 的 位 置 为 : 
P =P +%P 
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图 3.14 坐标 变换 综合 (平移 十 旋转 ) 示 意图 图 3.15 坐标 变换 综合 (旋转 十 平移 ) 示 意图 


这 里 需要 注意 的 是 "P 天 ,因为 对 于 坐标 原点 O, , P 是 在 坐标 系 {c} 中 描述 的 , 它 是 
未 知 的 ,而 ~“P 是 在 坐标 系 个 中 描述 的 , 它 是 已 知 的 。 

然后 ,考虑 坐标 系 { 让 和 坐标 系 {c) ,它们 是 纯 旋转 关系 , 则 点 卫 在 坐标 系 人 中 的 位 置 为 : 

iP =iR'P =IRGP +9 P) =iRIP HRP =iRiP +P 

上 式 中 ,由 于 坐标 系 {c) 与 坐标 系 {站 姿态 相同 ,所 以 及 二 :R 的 代 换 。 

例 5: 已 知 坐 标 系 17) 初 始 位 姿 与 坐标 系 11} 重 合 , 首 先 坐 标 系 47} 相对 于 坐标 系 { 寻 的 X 
轴 转 45°, 再 沿 坐 标 系 {i} 的 Y 轴 移 动 10。 假 设 点 己 在 坐标 系 17)} 的 位置 为 了 = 
[10,10,10] , 求 它 在 坐标 系 { 引 中 的 位 置 P。 

解 : 

经 过 一 个 绕 六 轴 的 转动 ,坐标 系 {j} 与 坐标 系 { 引 的 Y 轴 和 Z 轴 的 指向 不 同 ,再 经 过 一 
个 沿 坐标 系 { 寺 的 工 轴 的 移动 , 则 两 个 坐标 系 原点 不 同 。 

可 利用 式 (3-5) 求 点 己 在 坐标 系 4z} 中 的 位 置 。 

ip =IRIP +IP = R(X ,45¥P + P(Y,10) 


10 0 
10 0 


1 0 0 
cos45° —sin s 
0 sin45” cos45° 


10 
= | D 
10/2 


3.4 齐 次 坐标 与 齐 次 变换 


齐 次 坐标 可 用 于 表征 机 器 人 的 位 姿 , 齐 次 变换 可 用 于 实现 不 同 坐标 系 中 机 器 人 位 姿 的 
转换 ,利用 齐 次 坐标 和 齐 次 变换 研究 机 器 人 的 位 姿 间 题 十 分 便捷 。 
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3.4.1 齐 次 坐标 I . 


齐 次 坐标 是 一 种 用 于 投影 几何 的 坐标 表示 形式 ,类 似 于 用 于 欧式 几何 的 笛 卡 儿 坐 标 。 
齐 次 坐标 除了 用 在 机 器 人 学 中 ,也 是 计算 机 图 形 学 的 重要 工具 之 一 ,由 于 它 既 能 够 用 来 区 分 
向 量 和 点 ,也 更 易于 进行 仿 射 变换 (Affine Transformation) ,因此 通过 矩阵 与 向 量 相 乘 的 一 
般 运 算 可 有 效 地 实现 坐标 平移 .旋转 、 缩 放 及 透视 投影 。OpenGL 与 Direct3D 图 形 软件 以 
及 图 形 卡 均 利 用 齐 次 坐标 的 特点 ,以 4 个 暂 存 器 的 向 量 处 理 器 作为 顶点 着 色 引 擎 。 

1. 点 的 齐 次 坐标 

在 笛 卡 儿 坐 标 系 中 ;点 的 位 置 通常 用 3 维 列 向 量 表示 ,这 三 个 元 素 对 应 着 该 点 在 X、Y、 
Z 三 个 坐标 轴 上 的 投影 ,例如 点 的 位 置 可 表示 为 : 
P= [y 

点 的 齐 次 坐标 就 是 将 笛 卡 儿 坐 标 系 下 点 的 3 关 1 维 位 置 列 向 量 , 用 一 个 4X1 维 的 列 向 量 
表示 ,由 此 构建 一 种 新 的 表达 形式 ,这 增加 的 1 维 元 素 是 任意 的 非 零 实数 。 点 的 齐 次 坐标 可 表 
示 为 : 


其 中 心 是 非 零 的 实数 ,全 一 z, 全 一 y, 和 一 =。 所 以 w 实 际 上 是 一 个 非 零 的 比例 因子 , 它 对 笛 
卡 儿 坐标 系 中 点 的 位 置 坐标 进行 了 缩放 。 

对 于 笛 卡 儿 坐 标 系 下 点 的 位 置 P=[1 2 3], P= [1 2 3 1 # P= 
[2 4 6 2]' 都 是 该 点 位 置 的 齐 次 坐标 。 需要 指出 的 是 ,由 于 比例 因子 o 可 为 任意 的 非 
零 实数 , 因 此 一 个 点 位 置 的 齐 次 坐标 有 无 穷 多 个 。 为 了 简化 计算 及 便于 转换 ,在 机 器 人 学 
中 ,通常 取 比例 因子 。=1, 即 点 的 齐 次 坐标 的 通 式 为 : 

P=[x y < 1]'。 

坐标 系 原点 的 齐 次 坐标 为 : [0 0 0 1]7。 

2. 坐标 轴 的 齐 次 坐标 

第 卡 儿 坐 标 系 的 坐标 轴 是 具有 确定 方向 的 指向 无 穷 远 的 矢量 ,每 个 坐标 轴 的 齐 次 坐标 
可 采用 如 下 通 式 表示 : 


[a b c 0]', 其 中 a,b,c 称 为 方向 数 
笛 卡 儿 坐 标 系 的 三 个 坐标 轴 的 齐 次 坐标 表示 如 下 : 
(1) X 轴 的 齐 次 坐标 为 : [1 0 0 0]'; 
(2) Y 轴 的 齐 次 坐标 为 : [0 1 0 0]'; 
(3) Z 轴 的 齐 次 坐标 为 : [0 0 1 or. 


3.4.2 AREH 
如 图 3. 16 所 示 EER (JAR FERR (i) WINER 与 平移 向 量 *P 可 构成 一 个 
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| iR ip 
iT = ; (3-6) 
Dra Q T sa 


齐 次 变换 矩阵 江 的 含义 是 坐标 系 {7} 相 对 于 坐标 系 (让 的 位 姿 。 即 在 式 (3-6) 这 样 一 个 
单一 的 4X4 矩 阵 中 , 既 表 示 了 坐标 系 {7} 相 对 于 坐标 系 {z 的 位 置 ,也 表示 了 坐标 系 {7} 相 对 
于 坐标 系 {2 的 姿态 。 

例 6: 坐标 系 { 革 和 17) 的 相对 位 置 和 坐标 轴 指 向 如 图 3. 17 所 示 ,坐标 系 17} 的 原点 在 坐 
标 系 { 芭 中 的 位 置 为 [一 3, 一 4,5], 写 出 齐 次 转换 矩阵 ;T。 


4X4 的 齐 次 变换 矩阵 : 


图 3.16 齐 次 变换 示意 图 图 3.17 齐 次 变换 矩阵 表示 


解 
解 该 题 的 关键 是 写 出 旋转 矩阵 'R。 由 于 坐标 系 {7} 的 XY、2Z 三 个 坐标 轴 分 别 与 坐标 系 


{四} 的 Y.Z、X 三 个 坐标 轴 平 行 ,所 以 可 知 对 应 的 三 个 夹 角 为 0, 则 其 余弦 为 1, 再 由 于 ;R 的 
每 列 都 是 单位 向 量 , 所 以 每 列 的 其 余 两 个 元 素 必 为 0, 由 此 可 快速 写 出 ;iR 矩阵 的 每 列 。 
从 而 可 求 得 齐 次 转换 矩阵 !T 为 ， 


0 = 

1 0 @ —43 
ir = 

9 1 Q 9 

0 O O 1 


式 (3-6) 所 示 的 齐 次 变换 矩阵 通 式 也 能 表示 纯 平移 和 纯 旋 转 两 种 坐标 系 变换 ， 
了 sxs bi 


(1 
Ü 0 @ 1 


w. 


MNRE: Trans) = | 


1R 
(2) 纯 族 转 的 齐 次 变换 矩阵 ; RotCK,O) 一 | ， j 


TE HEE F HO FE WK IE a yB |: rh , H FABER Z (DRIER Z ( ; ) BJ 2 25 AR TE], Br LA Jë FE EE REIR 
为 单位 阵 。 而 在 纯 旋转 的 齐 次 变换 矩阵 中 ,由 于 坐标 系 {z) 和 坐标 系 {7)} 共 原点 ,所 以 平移 向 
EOP 为 零 ,这 里 K 表示 过 原点 的 单位 和 失 量 ,0 EARE K 旋转 的 角度 ,IR 与 K 和 0 的 关系 
式 在 3.5 节 介绍 。 
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利用 纯 平 移 和 纯 旋 转 的 齐 次 变换 矩阵 ,也 可 以 表示 两 坐标 系 之 间 既 有 平移 又 有 旋转 的 
复合 变换 : ii 


Rar- $P İR 0 
‘T= Trans P)Rot(K,0) = = : | | | 
epo ao 0.0. 1 


iR op 
-| | ED 
goa 1 


式 (3-7) 可 以 表示 两 种 不 同 的 坐标 系 变 换 : 相对 于 动 坐 标 系 的 先 平移 后 旋转 (如 
图 3. 18(a) 所 示 ) 和 相对 于 定 坐 标 系 的 先 旋转 再 平移 (如 图 3. 18(b) 所 示 ) 。 


(a) 平移 + 旋转 示意 图 (b) 旋转 + 平移 示意 图 


图 3.18 坐标 系 变换 示意 图 


对 于 第 一 种 坐标 系 变换 ,如 图 3. 18(a) 所 示 , 以 坐标 系 { 引 作为 参考 系 , 平 移 坐 标 系 {i) 至 
坐标 系 {站 的 原点 O, 产生 坐标 系 {c}; 然后 坐标 系 {c} 以 自身 为 参考 系 ,旋转 至 坐标 系 17) 。 
由 于 该 系列 坐标 系 旋转 属于 绕 动 坐标 系 旋转 ,所 以 齐 次 变换 矩阵 相 乘 的 顺序 与 坐标 系 旋转 
的 次 序 相 同 , 则 可 得 式 (3-7) 。 

对 于 第 二 种 坐标 系 变换 ,如 图 3. 18(b) 所 示 ,坐标 系 {z} 以 自身 作为 参考 系 , 旋 转 至 与 坐 
标 系 { 尹 姿态 相同 产生 坐标 系 {c} ,坐标 系 {c) 与 { 共 原 点 ; 然后 坐标 系 {c} 以 坐标 系 { 为 参 
考 系 ,平移 至 坐标 系 {j) 。 由 于 所 有 的 坐标 系 旋转 都 是 相对 于 定 坐标 系 { 寺 的 ,所 以 齐 次 变换 
矩阵 相 乘 的 顺序 与 坐标 系 旋转 的 次 序 相反 ,同样 可 得 式 (3-7)。 

这 个 例子 说 明 : 同样 的 齐 次 变换 矩阵 相 乘 关 系 式 可 以 表示 不 同 的 坐标 系 转换 关系 。 

假设 点 在 坐标 系 {让 ) 中 的 笛 卡 儿 坐 标 为 呈 , 其 齐 次 坐标 为 P=| ,| 


4X1 


kq 
假设 点 了 在 举 标 系 {让 中 的 笠 卡 儿 兴 标 为 ,其 齐 次 坐标 为 P' 一 | ”| 。 


则 : 
R HP IPP iR:P 十 史书 P ; 
es wd | 1 Suare 
pop a rhi 1 1 


上 式 中 ,由 笛 卡 儿 坐 标 系 下 的 坐标 变换 公式 (3-5) 得 P 了 二!R’'P 十 %P。 
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因此 ,点 了 的 齐 次 坐标 变换 的 计算 公式 为 : 
P' =!TP’ (3-8) 

式 (3-8) 也 被 称 为 齐 次 变换 通 式 , 该 公式 可 实现 任意 两 个 笛 卡 儿 坐 标 系 之 间 点 的 位 置 
变换 。 

例 7: 已 知 坐 标 系 {7 } 初 始 位 姿 与 坐标 系 {z)} 重 合 ,首先 坐标 系 (j) 相对 于 坐标 系 i) 的 
X 轴 转 45", 再 沿 坐 标 系 { 让 的 Y 轴 移 动 10, 再 沿 坐 标 系 {让 的 Z 轴 移 动 一 10V2 。 假 设 点 P 
在 坐标 系 {7} 中 的 位 置 为 二 [10,10,10]' , 求 它 在 坐标 系 {i} 中 的 位 置 'P。 

解法 1: 

因为 由 坐标 系 {i} 变 换 到 坐标 系 {j} 的 所 有 运动 都 是 相对 于 定 参 考 系 {i} 的 ,所 以 把 表 
示 各 个 运动 的 齐 次 矩阵 按 着 与 运动 次 序 相 反 的 顺序 相 乘 ,可 得 坐标 系 47 } 到 坐标 系 {2z} 的 齐 
WF BEET ,由 齐 次 变换 公式 (3-8) 可 求 得 点 P EBRA (i PHM. 

P'i=iTP’ = Trans(Y ,Z)Rot(X ,45°) > Pi 


1 0 0 0 1 0 0 01 [10 
Ga T Q 10 0 cos45° 一 sin45” 0||10 
u ò © 1 —10/2 | 0 sn45” eos45” 1|IW 
0 0 0 1 0 0 0 1J Lı 
t. Sa ¿Q 0 
10 10 
à k: -4 ii ioj |10 
10 0 
0 3 S —10v2||， i 
Sü ð 1 
所 以 ,点 卫 在 坐标 系 {z)} 中 的 位 置 为 卫 =L10,10,0】 。 


解法 2: 
由 于 坐标 系 {7 } 与 坐标 系 {z} 是 简单 的 旋转 和 平移 关系 ,所 以 可 以 直接 写 出 齐 次 转换 矩 
阵 订 ,然后 利用 齐 次 坐标 变换 公式 (3-8) 求 得 了 。 


P'=iT + P' 
1 0 0 0 10 
0 cos45° 一 sin45” 10 10 
= 0 sin45° cos45° — 1042 | |10 
0 0 0 | 1 
1 0 0 0 
10 10 
TP a 000. Y fio 
o 2 Š 1o j 
Ó o * @ 1 


同样 可 得 点 卫 在 坐标 系 {i} 中 的 位 置 为 P= 二 [10,10,0]"。 
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3.5 旋转 变换 通 式 | š 


前 面 介绍 的 坐标 系 旋转 都 是 绕 坐 标 轴 转 的 ,下 面 介绍 一 种 更 一 般 的 情况 : 坐标 系 绕 过 
原点 的 单位 矢量 旋转 ,主要 介绍 旋转 矩阵 通 式 以 及 等 效 转轴 、 等 效 转角 等 内 容 。 


3.5.1 旋转 矩阵 通 式 


问题 : 如 图 3. 19 所 示 ,坐标 系 { 让 } 绕 通过 原点 的 任意 单位 矢量 K 二 ki 十 kj 十 k.k 旋转 0 
角 得 到 坐标 系 {)) ,定义 旋转 和 矩阵 R(K,0) =R , 求 该 旋转 和 矩阵。 
解 : 


定义 两 个 坐标 系 人 {i} 和 {7 ) ,人 {让 与 {i ) 固 连 ,{j} 与 {7 ) 固 连 ; {i } 和 {7 ) 的 Z 轴 与 拓 量 K 
重合 ; 旋转 前 , {让 与 {j} 重 合 , (i) 与 {7 } 重 合 ,如 图 3.20 所 示 。 


3.19 绕 过 原点 的 单位 矢量 旋转 示意 图 图 3.20 绕 过 原点 单位 矢量 旋转 (假设 两 个 坐标 系 ) 


由 于 矢量 天 已 知 , 所 以 由 上 述 假 设 可 得 到 : 


n Oz di ny Oz Ë 

lm — p. — w 
iR =R = |n, o, a,|= |n, o, k, 
f Or fi s s ka 


WREEK U) 56 K FE 0 fi. WARR OXF (AI Z hab je 0 角 。 对 于 四 个 坐标 系 
GRE G G ) ,可 构建 如 图 3. 21 所 示 的 旋转 变换 关系 图 。 


由 该 关系 图 可 得 到 等 式 : i 
R(K,0) =R 
一 'R RJR 
=/R + R(Z,0) ¿R 7 j 


=R . R(Z,0) .7RT 图 3.21 旋转 变换 关系 图 


no Op -E cosO —sin0 0 $ ny fe 
ny Oy kyls |sin0 cosh © üs Öp s 
“ye ka 0 0 1 Ba By è 


利用 旋转 矩阵 的 单位 正 交 性 质 得 : 


(1) m。1 一 0。0 一 4。4 一 ]; 


所 以 : 


(2) n* 0=0* a=a ° n=0; 
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(3) a=n>o, 
假设 : 
s0= sin0,c0 = cos0,vers0 = (1 — cos0) 
整理 得 到 : 
k,kzvers9 +c0 k,k;uoers0Q —k.s0 kkversh 十 RSO 
R(K,0) = |kkyversO Rk.s0 k,k;,oersQ+c0 kk,vers0 —k.s0 (3-9) 
kkversO — kys0 kkversG+ ks0 kkvers0t c0 
式 (3-9) 就 是 绕 过 原点 的 单位 矢量 K 旋转 0 角 的 旋转 变换 矩阵 通 式 。 
在 前 面 介绍 过 坐标 系 绕 坐 标 轴 X、Y、Z 旋转 0 角 的 旋转 矩阵 ,实际 上 每 个 坐标 轴 也 是 一 
个 过 原点 的 单位 矢量 ,因此 也 可 以 采用 通 式 (3-9) 来 表示 各 个 旋转 矩阵 。 

(1) 绕 XX 轴 旋转 9 角 的 旋转 矩阵 ， 


1 0 0 

k=l; k,=k,=0, R(K.0)=|0 cos? —sin0 
L0 sinf cosô |] 
(2) # Y 轴 旋 转 9 角 的 旋转 矩阵 : 
[| cos0 0 sin] 


k=], 下 二 双人 一 0 1 0 


N À 


[—sin@ O cos] 


(3) 2 Z 轴 旋 转 9 角 的 旋转 矩阵: 


I cos —sin0 0] 
k,=1, E,=k;,=0, R(K,0)= sim cos 0 
0 0 1] 
BJ 8: 坐标 系 {7) 与 坐标 系 人 i} 重合 ,将 坐标 系 {7) 绕 过 原点 0 的 矢量 K 一 一 语 一 后 
二 4, 转 动 4 一 240", 求 旋转 矩阵 RCK,240°)。 
J5 
解 : 
HRE KTI: k =L, =L, 
A y V3 
和 s B 
cos240 = 2° sin240 = 2 ° vers 240 2 
将 上 述 值 代入 旋转 矩阵 通 式 (3-9) ,得 到 : 
a f 
JY. š L 1. Š a 4 全 
-ak IPE ki. D 人 
而 二 二 EEE 2 3: dr El 
R(K,240°)= |7 X $ zx > ) TS w RT 5? 
a 本 本 是 这 
ç S a AP te ba. ITE 
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3.5.2 等 效 转轴 与 等 效 转角 


问题 : 如 果 已 知 一 个 旋转 矩阵 R, 能 否 求 得 对 应 的 过 坐标 原点 的 矢量 K 和 转角 0? 
解 : 
这 个 问题 被 称 为 等 效 转轴 和 等 效 转角 问题 。 前 旋转 答 阵 
面 已 经 介绍 过 : 已 知 过 原点 的 矢量 K Mpo PAMA 。 正 : ox j 


利用 旋转 矩阵 通 式 (3-9) 求 得 对 应 的 旋转 矩阵 ,而 现在 (K,0) 
这 个 问题 正 是 一 个 反问 题 , 如 图 3. 22 所 示 。 对 于 该 问 
题 ,可 建立 式 (3-10) 所 示 的 矩阵 等 式 , 等 式 右边 矩阵 已 ”图 3.22 转轴 和 转角 与 旋转 矩阵 之 间 的 
知 , 左 边 矩 阵 未 知 。 由 于 等 式 的 两 边 都 是 矩阵 ,因此 这 转换 关系 
个 问题 实际 上 就 是 一 个 矩阵 方程 求解 问题 。 

k.k.vers0g+-c0 k yk:versô—k.s0 k.k.vers0+ k,s0 He O (Q; 

: -| i am 


k.k,vers0+ ks0 ARyversO 十 c0 kk,vers0— k,s0 
zkeversO— RJs0 Rk;,jk.vers0+- ks0 k.k.vers0—+ co 
对 于 式 (3-10) ,具体 的 求解 步骤 如 下 : 
首先 ,将 式 (3-10) 两 边 矩 阵 的 主 对 角 线 元 素 分 别 相 加 , 则 得 : 
n, +o, + a. = (k: +k? + E2)vers0+ 3c0 = 1 + 2c0 
整理 得 到 : 


ny My a 


Wz Os ne 


cosg = > (n, +o, +a, — 1) (3-11) 
其 次 ,将 式 (3-10) 两 边 矩 阵 关 于 对 角 线 对 称 的 元 素 成 对 相 减 得 到 : 


o, 一 4y = 2k,sin0 
a, —n, = 2k,sinĝ 
n, — o, = 2k.sin0 
将 上 列 等 式 两 边 平方 相 加 ,得 : 
(o, — a,)° == (a, —n,)° + G, = o;)° = 4 sin“0 


由 上 式 可 得 : 
sinf =+ 3 af Os =a)" + (a, =n) + (n, — s, )° (3-12) 
由 求 得 的 sin0 可 得 : 
= 9x — Gy = s: U fs — n, — Oz >. 
k. 2sin0 ” š, 2sin0 ' É. 2sin0 bas 
由 此 可 求 得 等 效 转轴 K. 


利用 求 得 的 sin 和 cos0 可 得 : 
2 Co, ma Tear ny T (m; —@, Y° 


tang 一 十 Z Po, tas] 
采用 反正 切 函 数 可 求 得 等 效 转角 0: 
p= arctan( + (o, —a,) F (as —n 2) t (ny — 0,35 ) (3-14) 
Wy Di = 


由 此 求 出 了 旋转 矩阵 对 应 的 等 效 转轴 和 等 效 转角 ,它们 都 有 两 个 解 。 
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值得 注意 的 是 ,对 于 给 定 的 旋转 和 矩阵, 一般 对 应 的 等 效 转轴 K 和 等 效 转角 9 不 是 唯 
一 的 。 
例 9: 求 复合 旋转 矩阵 六 =RCX,90)RCY,90) 的 等 效 转轴 天 和 等 效 转 角 0。 
解 : 
(1) 首先 求 出 旋转 矩阵 : 
iR= R(X,90)R(Y ,90) 


1 0 0 © ü 1 Ó ü 1 n: s Ba 
0 0 十 E 1 | ; 0 . - o, | 
t g —1 0 0 0 1 O i í 


(2) 利用 式 (3-12) 求 sin0; 


sin=—+ x V (o, —a,) + (a, — n,.)° + (n, — o,)° 


= 1== 0 - + 
(3) 利用 式 (3-13) 求 等 效 转轴 K: 


L Gy — = 2 V3 == + 
= (Ü [2 x+ 2) £ 
Q QI _- sa o, V3 \=— N 
ky = 2sin0 =m [2 <+ >) = 
一 —— O S oA V3 === 1 
k. = B2 = CC 1-0) + [2 x+ Š) 二 二 
所 以 等 效 转 轴 有 两 个 : 
je AR e, m ars. 3 
K, =—¿L—;+—k, K,=——;— b 
机 
(4) 利用 式 (3-11) 求 cos0: 
cosg = +n, +0, +a, —1) 一 去 (0 十 0 十 0 一 1) 一 一 去 
(5) 利用 式 (3-14) 求 等 效 转角 0: 
3 
; +Š 
tanb = sing - 2 3 
(ie .7 — _ 1 
2 


由 于 cos9== 一 六 ,所 以 0 只 能 在 二 ,三 象限 , 故 等 效 转角 有 两 个 : 0, —120°,0, —240°, 
所 以 求 得 的 等 效 转轴 和 等 效 转角 有 两 组 解 , 即 ; 


: 95 ya j! ð 
K, = iF j4 k, @ = 120 


K, = 


EA sa je. 1] ° 
= k, Q = 240 
站 +S s 
由 上 述 计 算 可 知 : 有 两 组 过 原点 的 矢量 K 和 转角 9 的 组 合 与 绕 坐 标 轴 X 和 YY 的 复合 
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转动 等 效 。 这 两 组 解 的 关系 是 互 为 相反 数 , 即 =K ,0 一 一 。 可 对 照例 8 理解 。 

由 此 可 推广 得 出 如 下 结论 : 任何 一 组 绕 坐 标 轴线 的 复合 转动 总 等 效 于 绕 过 原点 的 某 一 
RE K #k2J 0 角 。 简 而 言 之 ,任意 数量 的 绕 坐 标 轴 转动 的 组 合 可 被 绕 过 原点 的 某 一 矢量 转 
动 一 个 角度 代替 。 


3.6 齐 次 变换 通 式 


问题 : 如 图 3. 23 所 示 ,假设 单位 矢量 K= ki tkj 十 k.k A P= psi 十 pyj + b.k KR 
HRE K # 0 角 的 齐 次 变换 矩阵 。 

解 : 

这 里 讨论 的 是 一 种 更 具 一 般 性 的 问题 , 即 单位 矢量 K 没有 过 坐标 系 原点 的 约束 。 

定义 两 个 坐标 系 {i } 和 {7 ) ,它们 的 坐标 原点 在 点 , {让 与 {i } 固 连 ,{j) 与 {7 BE; 
{i 和 {7 的 坐标 轴 分 别 与 { 字 和 {站 的 坐标 轴 平 行 , 旋 转 前 , (让 与 {) 重 合 , (i) 与 {7} 重合 。 

可 构建 如 图 3. 24 所 示 的 旋转 变换 关系 图 。 


i +T J 
Yä İT 
I J 
I 
I 
x i IT J 
图 3.23 坐标 齐 次 变换 示意 图 图 3.24 旋转 变换 关系 图 


由 此 旋转 变换 关系 图 可 求 得 下 列 变换 等 式 : 


i TT 
由 于 
; Ix P 
;T = Trans(P) = | i 
O01ixs 1 
, I; =P 
¿T =; T = Trans(— P) = | |. 
Ox 1 
à R(K.0) O; 
rT = Rot(K,0) = | ] 
Oixs 1 
最 后 可 求 得 : 


i R(K,0) —R(K,0)P+P 
JT = Trans(P)Rot(K,0)Trans(— P) = | | 


0i>xs 1 
由 此 可 得 坐标 系 绕 不 过 原点 的 矢量 天 旋转 的 齐 次 变换 矩阵 的 通 式 为 : 
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R(K,0) 一 RCIK,O)P 十 也 
ip = i l (3-15) 
Ois 1 
式 (3-15) 比 式 (3-9) 更 具有 一 般 性 ,几乎 可 以 处 理 任 意 类 型 的 坐标 系 旋 转变 换 问 题 , 而 
绕 过 原点 的 矢量 旋转 问题 只 是 绕 不 过 原点 的 矢量 旋转 问题 的 一 个 特例 。 


例 10: 坐标 系 {7) 与 坐标 系 {? 引 重合 ,将 坐标 系 { 刀 绕 过 点 P sus m j 
旋转 0 二 240",P' 二 [2 4 6], 求 旋转 矩阵 和 。 
fE: 
0 o 1 
(1) 由 式 (3-9) 可 得 : R(K,240)=|1 0 0 
0 1 0 
(2) 将 RCK,240") 和 Pi 一 [2 4 6] 代入 齐 次 变换 矩阵 通 式 (3-15) ,可 得 ， 
0 1 一 4 
T "0 Ó 2 
1. = 
0 Jj Ó 2 
G0 0 Ï 


对 于 上 述 这 个 旋转 矩阵 ,同样 存在 其 逆 问 题 , 即 已 知 该 旋转 矩阵 , 求 P 点 、 矢 量 K 和 转 
角 9。 一 般 情况 下 , 解 是 不 唯一 的 ,即使 在 矢量 KK 已 知 的 情况 下 ,点 了 的 值 也 不 唯一 ,所 有 的 
P 点 可 构成 一 条 直线 。 


3.7 机 器 人 姿态 的 其 他 表示 方法 


前 面 介 绍 了 用 3X3 HEER 表示 机 器 人 的 姿态 ,这 个 矩阵 有 9 个 元 素 ,6 个 约束 条 件 ， 
实际 上 只 有 3 个 独立 元 素 。 这 意味 着 尽管 表示 3 个 变量 ,但 在 计算 编程 时 需要 输入 9 个 元 
素 ,显然 很 不 方便 。 因 此 ,研究 者 们 也 提出 了 一 些 其 他 的 机 器 人 姿态 的 表示 方法 ,只 采用 
3 个 元 素 表 示 机 器 人 姿态 。 在 这 些 姿 态 的 表示 方法 中 ,RPY 角 、 欧 拉 角 和 四 元 数 的 应 用 最 
普遍 ,下 面 对 它 们 作 介 绍 。 


3.7.1 RPY 角 


RPY 角 是 船舶 在 海上 航行 时 常用 的 一 种 姿态 表示 方法 ,其 笛 卡 儿 坐 标 系 建立 方法 如 
F: 以 船 头 前 进 方向 为 Z 轴 ,以 垂直 于 甲板 平面 的 法 线 向 上 方向 为 X 轴 ,Y 轴 依 据 右手 法 
则 由 X Z 确定 ,一般 平行 于 甲板 平面 指向 右 航 外 侧 , 如 图 3. 25 所 示 。 定 义 绕 Z 轴 的 转动 为 
RoR) ,转角 为 a; 绕 Y 轴 的 转动 为 Pitch( 俯 仰 ) ,转角 为 p: 绕 X 轴 的 转动 为 Yaw( 偏 
航 ) ,转角 为 Y。RPY 的 名 称 来 源 于 Roll,Pitch, Yaw 三 个 单词 的 首 字母 。 

下 面 介绍 如 何 用 RPY 角 描 述 船舶 的 姿态 。 

为 了 描述 船舶 的 姿态 ,需要 建立 两 个 笛 卡 儿 坐 标 系 ,这 两 个 坐标 系 共 原点 。 其 中 一 个 坐 
标 系 为 参考 系 (让 , 男 一 全 是 船体 坐标 系 {7} 。 这 两 个 坐标 系 的 区 别 是 : 参考 系 {i} 与 大 地 固 
连 , 它 的 姿态 是 固定 的 ; 而 坐标 系 {j}) 与 船体 固 连 , 它 的 姿态 与 船体 是 一 致 的 , 随 着 船体 的 运 
动 而 变化 。 
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图 3.25 RPY 角 描述 船舶 航行 时 的 姿态 


坐标 系 {j} 相 对 于 参考 系 { 让 的 姿态 描述 为 : 开始 时 ,坐标 系 42) 和 坐标 系 {7} 重 合 ,坐标 
# U) ËB 5 X, tey 角 , 再 绕 Y; 转 B 角 , 最 后 绕 Z, 转 a fli. 
很 显然 ,这 是 一 个 绕 固定 坐标 系 的 多 个 坐标 轴 旋 转 的 问题 。 依 据 前 面 介 绍 过 的 绕 固定 
坐标 系 的 多 个 坐标 轴 旋 转 的 旋转 矩阵 的 求法 ,可 得 坐标 系 { 刀 相对 于 坐标 系 { 寻 的 姿态 矩阵 为 : 
İR p (yy8,a 一 RCZ,a)RCY,p)RCX，7) 


ca ~ 0 cB o sg) 9 0 
= sa ca 0 0 L 0 o y — $y 
0 0 Fj =s Ü ello y ey 


sacB sasßsy 十 cacy sasßcy — casy 
R c psy cpcy 
由 上 述 计算 过 程 可 以 看 出 ,如 果 已 知 绕 着 固定 参考 坐标 系 的 X .Y .Z 三 个 坐标 轴 的 转 
角 y,B,a, 则 可 以 利用 上 述 通 式 求 出 一 个 3X3 的 姿态 矩阵 表示 船舶 的 姿态 ,而 且 这 个 矩阵 
是 唯一 的 。 同 理 , 也 可 以 用 RPY 角 表 示 其 他 刚体 的 姿态 ,如 机 器 人 、 飞 机 等 。 
RPY 角 与 姿态 矩阵 之 间 也 存在 着 一 个 反问 题 : 如 果 已 知 姿态 和 矩阵, 如何 求 RPY f? 
依据 此 问题 可 列 出 下 列 和 矩阵 等 式 , 等 式 左边 矩阵 已 知 。 
sach sasBsy 十 cacy sasÑBcy — casy 


ri rl na 
31 Vass T33 =s chpsy cpcy 
很 显然 ,这 是 一 个 矩阵 方程 的 求解 问题 。 其 解法 如 下 : 


首先 ,将 等 式 两 边 和 矩阵 中 的 (1,1) 和 (2,1) 两 个 元 素平 方 后 求 和 ,得 : 
(cac p)? + (sac B)? = co’! h = rii + ri 


cach caspsy —sacy casBcy + sasy 
= CƏ= 162 


cacB casBsY —sacy casßcy + sasy 


可 得 : 
cosg= + Vri Tri 
如 果 知 道 8 的 取 值 范围 , 则 可 方便 地 确定 cosp 的 正 负 ,从 而 采用 反 余弦 方法 求 得 8。 假 
i& —90°<8<90°, M] cosg= yra Fria ,但 8 会 有 两 个 解 。 
这 里 ,介绍 另外 一 种 求 角 度 的 方法 : 双 变 量 反 正切 函数 法 。 双 变量 反正 切 函 数 的 形式 
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为 : Atan2(y,z)=arctan ,y=sing, z=cosp. 该 函数 的 优点 是 能 根据 x 和 的 符号 唯一 


地 确定 对 应 的 角度 值 。 例 如 Atan2(—2,—2)= —135°,Atan2(2,2)=45°, 
基于 该 方法 ,三 个 角度 的 求解 方法 如 下 : 
如 果 cosB 才 0, 则 : 


B= Atan2(— ra sy rir + rir) 
y= Atan? (rz T33) 
如 果 cosg=0=B8= 90°, ni$: B= 90°, r =sin(y—a) rz =cos(y—a) , I : 


! = Atan2 (rz sri) 


a = 0 
B= srd 
y = Atan2(rs yr ) 
a = 0 
p=—90" 
y =— Atan2(riz sT22) 


很 显然 ,如 果 已 知 一 个 姿态 矩阵 ,其 对 应 的 RPY 角 是 不 唯一 的 。 


3.7.2 Wi fs 


欧 拉 角 (Euler Angles) 是 瑞士 数学 家 Leonhard Euler(1707—1783 年 ) 提 出 的 一 种 采用 
绕 运 动 坐标 系 的 三 个 坐标 轴 的 转角 组 合 描 述 刚 体 姿 态 的 方法 ,与 RPY 角 类 似 , 也 是 采用 了 
三 个 角度 变量 。 该 方法 被 广泛 用 于 数学 、 物 理学 、 航 空 工程 及 刚体 动力 学 。 

欧 拉 角 有 多 种 类 型 , 绕 不 小 于 两 个 坐标 轴 的 三 个 转角 的 组 合 都 可 表示 成 欧 拉 角 ,如 坐标 
轴 组 合 为 ZX-Z,ZY-Z,Y-X-Y,Y-Z-Y, X-Y-X , X-Z-X , X-Y-Z ,所 以 欧 拉 角 表示 刚体 姿态 
的 时 候 需 要 与 关联 的 坐标 轴 组 合 , 以 表明 旋转 的 坐标 轴 及 旋转 顺序 。 下 面 介绍 两 种 常用 的 
欧 拉 角 。 

1. ZYX 欧 拉 角 

ZYX 欧 拉 角 描述 姿态 的 方法 如 下 : 

有 两 个 坐标 系 { 让 和 {7}) ,开始 时 ,坐标 系 { 引 和 坐标 系 {7} 重 合 ,坐标 系 {j} 首 先 绕 2Z;/ Z, 
转 a 角 , 再 绕 Y; 转 8 角 ,最 后 绕 X, Eey 角 。 

很 显然 ,这 是 一 个 绕 动 坐标 系 的 多 个 坐标 轴 旋 转 的 问题 。 依 据 绕 动 坐标 系 的 坐标 轴 旋 
转 的 矩阵 连 乘 方法 , 则 可 求 得 坐标 系 { 尹 相对 于 参考 系 { 坟 的 姿态 描述 矩阵 如 下 : 

IR... Cab = R(Z,a)R(Y ,BR(X,Y) 


ca —sa 0 eB Ó s8J[1 0 0 
= se a 0 0 1] 0 9 ë — y 
0 0 L] — s O ¿81 00 ç “ey 


cacB casBsy —sacy casBcy + sasy 
sacB sasBsY 十 cacy sasÜBcy — casy 
— sp c Bsy c8cy 

这 样 ,就 实现 了 用 三 个 绕 动 坐标 轴 的 转角 来 表示 刚体 的 姿态 。 
现在 回头 看 看 RPY 角 表 示 的 姿态 矩阵 , 见 式 (3-16) ,再 看 看 上 面 这 个 ZYX 欧 拉 角 表 


(33 
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示 的 姿态 矩阵 ,很 显然 它们 是 相同 的 ,但 它们 绕 坐 标 轴 旋 转 的 顺序 是 相反 的 。 这 也 从 侧面 说 
明了 坐标 系 绕 定 轴 旋 转 与 绕 动 轴 旋 转 具 有 等 效 性 。 ` 
2. ZXZ 欧 拉 角 
ZXZ aai ia a a 
有 两 个 坐标 系 { 池 和 {7) AR ERROMARA UEA MRR U) B 25 Z Z 
转 a 角 ， iii 转 B 角 ,最 后 绕 Z; 转 y fa BRUTA R {i IIRA T : 
İR ae CasB) = R(Z,a)R(X,BR(Z,Y) 


ta — Om Ó 0 w — Q 
= |sa ca 0 | 0 <Ë —sB ç cy 0 
LO 0 1110 sp eg 0 1 


Suey — sap sy —qasy' == Saey sas 8 
= sacy + cacBsy — sasy + cacBcy — cash 
L sBsy sBcy ch 

ZYX 欧 拉 角 需要 用 到 三 个 坐标 轴 , 而 ZXZ 欧 拉 角 只 需要 两 个 坐标 轴 ,在 一 些 具体 的 
应 用 中 使 用 ZXZ 这 种 形式 的 欧 拉 角 会 更 方便 些 。 例 如 ,在 用 欧 拉 角 表 示 机 器 人 末端 的 
姿态 时 ,如 果 用 ZXZ 欧 拉 角 ,只 需要 考虑 Z、X 两 个 坐标 轴 即 可 ,而 不 需要 考虑 Y 轴 指 向 
哪里 。 

欧 拉 角 与 姿态 矩阵 之 间 也 存在 一 个 逆 问 题 , 即 已 知 姿态 矩阵 求 对 应 的 欧 拉 角 。 如 
式 (3-19) 所 示 , 等 式 右 边 的 姿态 矩阵 已 知 ， SSH, 很 显然 ,这 也 是 一 个 解 


(3-18) 


和 矩阵 方程 的 问题 。 
CRY — sacBsy — tasy — sacpey  sasB Ph fm ris 
sacy +cacpsy — sasy +cacpcy —casp| = |ra r> ~ (3-19) 
SBSY spBcy ch yaa T32 Tas 


对 于 式 (3-19) ,可 从 等 式 两 边 和 矩阵 的 元 素 (1,3) 和 (2,3) 找 到 求解 途径 ,结果 如 下 : 


a= Atan? (rs a — Fa) 


如 果 oa Mr3i Fr32 raa) 
y= Atan? (rz sraz) 


B=0 时 ,a 王 0,7y 王 Atan2( 一 ri SPd 
如 果 sing= 0.11 
8B=180 Rf a=0, y= Atan? (rizs — rn) 


很 显然 , 欧 拉 角 的 解 也 不 唯一 。 
3.7.3 四 元 数 


一 般 来 讲 , 用 欧 拉 角 表示 刚体 的 姿态 或 运动 是 非常 简单 有 效 的 ,但 是 在 某 些 特殊 的 情况 
下 , 欧 拉 角 会 出 现 所 谓 的 “万 向 节 死 锁 (Gimbal Lock)” 问 题 , 即 欧 拉 角 无 法 描述 刚体 的 运动 。 
出 现 万 向 节 死 锁 问 题 的 原因 是 采用 有 序 三 个 角度 的 欧 拉 角 方法 并 不 能 描述 所 有 的 刚体 
运动 。 

1. 四 元 数 的 定义 及 特点 

1843 年 ,爱尔兰 数学 家 William Rowan Hamilton 人 1805 一 1865 年 ) 在 研究 将 复数 从 描 
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述 二 维 空间 扩展 到 高 维 空间 时 ,创造 出 了 一 个 超 复数 : 四 元 数 (Quaternion) 。 四 元 数 能 表 
示 四 维 空间 ,由 一 个 实数 单位 1 和 三 个 虚数 单位 i\j、k 组 成 ,通常 表示 形式 为 : 
q = a + bi+ cj + dk (3-20) 
式 (3-20) 中 ,ac.d 均 为 实数 ,ij 被 称 为 第 一 、 第 二 、 第 三 维 虚 单位 ,具有 下 列 
性 质 : 
E | 
mim] kaisi keke 
可 以 看 出 ,i\j\k 的 性 质 与 笛 卡 儿 坐 标 系 三 个 坐标 轴 的 性 质 很 像 。 
为 了 表达 简便 ,通常 将 四 元 数 写成 一 个 实数 和 一 个 向 量 组 合 的 形式 : 
q = (asv ) = (a,b,c,d) 
上 式 中 ,vw 是 一 个 向 量 ,vw =bitcjtdk a bcd 为 4 个 有 序 的 实数 。 四 元 数 可 以 看 作 
是 一 种 实数 和 向 量 表达 的 一 般 形 式 ,实数 可 看 作 是 虚 部 为 0 的 四 元 数 ,而 向 量 可 看 作为 实 部 
为 0 的 四 元 数 , 也 被 称 为 纯 四 元 数 。 任 意 的 三 维 向 量 都 可 以 转化 为 纯 四 元 数 。 
四 元 数 具 有 下 列 特点 : 
(1) 可 以 避免 万 向 节 死 锁 ; 
(2) 几何 意义 明确 ,只 需 4 个 数 就 可 以 表示 绕 过 原点 任意 向 量 的 旋转 ; 
(3) 方便 快捷 ,计算 效率 高 ; 
(4) 比 欧 拉 角 多 了 一 个 维度 ,理解 困难 。 
四 元 数 在 机 器 人 学 .数学 ,物理 学 和 计算 机 图 形 学 中 具有 很 高 的 应 用 价值 。 
2. 四 元 数 的 运算 
四 元 数 是 一 个 新 的 超 复 数 , 针 对 它 的 计算 问题 ,Hamilton 给 出 了 四 元 数 的 加 法 、 乘 法 、 
逆 和 模 等 的 计算 规则 。 
£ q = (ai, ) ,qs = (a ,vs)。 
(1) 四 元 数 的 加 法 : 
q +q = (a, +a, sv +v) 
(2) 四 元 数 的 乘法 : 
qiq: = (araz — Vi * V saiv + az Hv X v) 
由 于 涉及 矢量 运算 ,四 元 数 乘法 不 适用 于 乘法 交换 律 , 即 q q. Zq; q 。 
(3) JEHA: 
q* = (a, —) 
(4) 四 元 数 的 逆 : 


(5) 四 元 数 的 模 : 
|q|= /q* q = vg q = VÆ + b° + e + d° 
3. 四 元 数 表 示 刚 体 姿态 及 运动 变换 
模 为 1 的 四 元 数 被 称 为 单位 四 元 数 。 对 于 单位 四 元 数 , 由 于 | | =1, 所 以 有 : 
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B: 
q'' = q' s (3-21) 
iX uJ KAK faj (k tB y pu gu 8k r E , 
一 个 单位 四 元 数 描述 了 一 个 转轴 和 绕 该 转轴 的 旋转 角度 ,因此 可 以 描述 刚体 的 运 
姿态 。 单 位 四 元 数 可 表示 成 如 下 形式 : 
q = (cos0 ,vsing) (3-22) 
该 四 元 数 表 示 绕 向 量 v 旋转 20 角度 的 运动 ,角度 为 零 表 示 刚 体 的 初始 姿态 ,不 同 的 角 
度 代表 着 刚体 相对 于 初始 姿态 的 新 姿态 。 这 里 ,o 是 过 坐标 系 原 点 的 任意 单位 向 量 。 
同 理 , 如 果 已 知 过 坐标 系 原点 的 单位 向 量 z 一 (0,2,c,d)( 纯 四 元 数 ) 和 绕 该 向 量 旋转 的 
角度 0, 则 表示 该 运动 的 单位 四 元 数 为 : 
q = (cos(0/2),sin(0/2)b,sin(0/2)c,sin(0/2)d) 
这 里 ,5b\c.d 为 向 量 v 在 笛 卡 儿 坐 标 系 的 X YZ 坐标 轴 上 的 分 量 。 
假设 一 个 向 量 w 绕 向 量 z 旋转 轴 角 度 0 至 vi , 则 wi 可 表示 为 : 


要 一 gid (3-23) 

例 11: 假设 点 P=(1,1,0) ,将 该 点 绕 旋 转轴 v = (1,0,0) 旋 转 90", 求 旋转 后 该 点 的 
坐标 。 
解 : 


首先 将 点 卫 表 示 成 纯 四 元 数 , 即 p= (0,P)=(0,1,1,0), 


由 式 (3-22) 得 q= (cos 45" ,vsin 45°) = pi 


2 0,0). 


由 式 (3-21) 得 q =q" = 2 o). 
2 2 
最 后 由 公式 (3-23) 得 : 
p - qpbq ` (Woo) na aig 这 o0) = (0,1,0,1) 


2 2 

所 以 旋转 后 该 点 的 坐标 是 (1,0,1)。 
4. 四 元 数 与 其 他 姿态 表示 方法 的 转换 
欧 拉 角 与 四 元 数 可 以 实现 相互 的 转换 ,旋转 矩阵 也 可 以 表示 成 四 元 数 。 
(1) 欧 拉 角 转换 为 四 元 数 
设 ZYX 欧 拉 角 为 (%,0,o) , 则 对 应 的 四 元 数 为 : 

w cos($/2)cos(0/2)cos(y/2) + sin($/2)sin(0/2)sin(y/2) 

x| |sin($/2)cos(0/2)cos(y/2) — cos(#/2)sin(0/2)sin(g/2) 
y| |c+osC(#/2)sin(0/2)cos(%/2) + sin(#/2)cos(0/2)sin(g/2) 
< cos($/2)cos(0/2)sin(y/2) — sin($/2)sin(0/2)cos(%/2) 
(2) 四 元 数 转换 为 欧 拉 角 
设 四 元 数 为 9 一 也 十 zi 十 yj 十 zk, 则 对 应 的 ZYX 欧 拉 角 为 : 


q = 


$ arctan2[2 (wr + yz) ,1 — 2(>z° +y°)] 
0|= |arcsin(2 (wy — zz )) 
Y arctan2[2 (wz + zy),1 — 2(2 +2z)] 
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(3) 旋转 和 矩阵 转换 为 四 元 数 


设 旋转 矩阵 为 : 
F Ti Ye 
R= Ë T22 ~ 
Fai fa Ta 
则 对 应 的 单位 四 元 数 为 : 
" + Vru Era Fra t] 
q = = sgn(ra — rz) Yru — rz — ra Fl 
7 sgn(7ra — ra) yra 一 mas 一 ma 十 工 
"AN —a h r Y 
3.8 人 小 结 


本 章 主要 介绍 了 机 器 人 位 置 和 姿态 的 数学 描述 方法 及 不 同 坐 标 系 变换 的 类 型 和 方法 ， 
最 后 介绍 了 机 器 人 姿态 的 其 他 表示 方法 : RPY 角 、 欧 拉 角 和 四 元 数 。 
总 结 补充 下 列 知 识 要 点 : 


P WAARAAN: 

@ 坐标 系 的 描述 : BERU PABP DER. 

O HRAT: 坐标 系 {j} 和 坐标 系 ( 由 之 间 的 位 姿 转换 算 子 。 

(2) 齐 次 变换 矩阵 工 的 相 乘 

齐 次 变换 矩阵 工 相 乘 的 顺序 一 般 不 可 换 , 但 当 坐标 系 变换 是 在 相同 参考 系 下 的 平移 或 
绕 同一 坐标 轴 的 旋转 时 可 换 。 

(3) 旋转 抢 阵 /姿态 矩阵 求 逆 

旋转 矩阵 或 姿态 矩阵 具有 特殊 的 属性 : 站 开 一 碎 7 一 遂 , 在 矩阵 求 送 时 可 灵活 运用 该 矩 
阵 的 特点 。 


R 
O 刘 交 变换 短 ET | 
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附录 线 固 定 坐 标 系 的 多 个 坐标 轴 转 动 的 
旋转 矩阵 计算 


如 图 3.12 所 示 ea £ i) 26 R: X 轴 旋 转 a 角 , 得 到 新 坐标 系 {m} ,坐标 系 (m) 再 绕 坐 标 
系 { 让 的 Z 轴 旋转 0 角 , 得 到 新 坐标 系 {7)} , 求 旋转 和 矩阵!R (a,0)。 
解 : 
这 个 推导 需要 用 到 绕 过 原点 的 矢量 天 的 旋转 矩阵 通 式 。 
(1) MRR R (i) X; 轴 旋 转 a 角 得 到 新 坐标 系 {m}) ,可 得 : P RS=RX a). 
(2) 将 Z, 轴 看 作 是 坐标 系 {m} 中 过 原点 的 矢量 K, 假 设 Z, ME X, ,Y, s Z, 三 个 坐标 轴 
上 的 投影 分 别 是 &,k,,k., 所 以 有 : 
K = k,i+ k,j+ k-k = 0 + i+ sina ° i + cosa * k 
WE 
kkvers0G +c0 k ,kversô— k.s kek.vers0O 十 RSO 
R = R(K,0) = |kkyversOT ks0 kËk;,kyoersQ-+c0 kkvers0 — k,s0 
k:k:versô 一 RSO RNReoery0 十 RSO kE.k.oers0 + c0 
Ki k, =0,k, =sing, k, = cosa, versf = (1 — cos ARA ER, T4: 


cos — cosasin0 sinasin0 
R(K.0) = | cosasin0 cos0-- (1 — cos0)sin”"a cosasina(] — cos0) 
— sinasin0 cosasina(1 — cos0) cosa (1 — cos0) + cos0 
因此 有 : 
iR ="RİR = R(X,a)R(K ,0) 
假设 : 


IR = R(X,,a)R(X,, = GRZ R(X ,a) = RZ ORX ra) 
只 要 证 明 R(K,O=R(X;,—a)R(Z R(X; a) ,结论 即 可 得 证 。 
£ Rxzx 二 R(X;, 一 a)R(2,,0)R(Xisa) ;计算 得 到 ， 


H! 0 0 eos — sime OJTI 0 0 
Rz; = Q tosa) — sim =a) | |Sing co 0] | cosa. — sina 
lO sin(—a) cos(— a) 0 0 1j [0 sina cosa 
cos0 — cosasin0 sinasin0 
及 xzx = | cosasin0 cos*acosh + sina cosasina (l — cos0) 
[一 sinasin cosasina(l— cos) cos au(1 — cos) + cos0 


cos0 — cosasin0 sinasin0 
R(K,0) = | cosasin0 cos*acos0 + sina cosasina(1 — cos0) 


— sinasin0 cosasina(l 一 cosg) cos*a(l 一 cosg) + cos0 


即 


R(K.0) = Rxzx =R(X;, a)R(Z,.0)R(X; sa) 
所 以 : 
:R = R(X; .a)R(K.0) = R(X; œ R(X; . —a)R(Z; ,0)R( X, ,0) = R(Z; ,.0)R( X; 9) 


机 器 人 运动 学 (Kiaematics) 是 从 几何 角度 描述 和 研究 机 器 人 的 位 置 .速度 和 加 速度 随 
时 间 的 变化 规律 的 科学 , 它 不 涉及 机 器 人 本 体 的 物理 性 质 和 加 在 其 上 的 力 。 本 章 重点 介绍 
机 器 人 运动 学 的 建 模 方法 及 逆 运 动 学 的 求解 方法 。 


4.1 引言 


机 器 人 运动 学 问题 主要 在 机 器 人 的 工作 空间 与 关节 空间 中 讨论 ,包括 正 运动 学 
(Forward Kinematics) 和 道 运 动 学 (Inverse Kinematics) 两 部 分 内 容 。 如 图 4.1 所 示 , 由 机 
器 人 关节 空间 到 机 器 人 工作 空间 的 映射 称 为 正 运动 学 ,由 机 器 人 工作 空间 到 机 器 人 关节 空 
间 的 映射 称 为 道 运动 学 。 正 运动 学 也 被 称 为 运动 学 建 模 , 而 逆 运 动 学 也 被 称 为 运动 学 求 逆 
或 求 逆 解 。 


4.2 运动 学 建 模 方法 


机 器 人 运动 学 建 模 方法 主要 包括 几何 建 模 法 .D-H 建 模 法 等 。 几 何 建 模 方法 适用 于 结 
构 简单 的 机 器 人 ,尤其 是 适合 平面 机 器 人 。D-H 建 模 方法 具有 较 强 的 通用 性 ,对 于 串联 机 
器 人 .并 联机 器 人 都 适用 。 其 他 建 模 方法 ,如 旋 量 法 、 四 元 数 法 等 , 则 各 有 侧重 。 本 节 重 点 介 
绍 三 角 几 何 建 模 方法 和 D-H 建 模 方法 。 
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4.2.1 三 角 几 何 建 模 方法 


如 图 4. 2 所 示 是 一 个 具有 单个 旋转 关节 的 机 械 臂 ,如 何 建立 它 的 运动 学 模型 呢 ? 
对 于 该 机 械 臂 ,首先 在 其 基 座 上 建立 一 个 参考 坐标 系 XOY, 定 义 关 节 变 量 0 为 从 X 轴 
至 臂 杆 的 夹 角 , 逆 时 针 方 向 为 正 , 顺 时 针 方 向 为 负 。 机械 臂 关节 空间 只 有 一 个 变量 0, 其 工 
作 空 间 具 有 两 个 变量 x 和 y ,表示 末端 抓 手中 心 在 参考 系 中 的 位 置 ,采用 三 角 几 何 的 方法 可 
以 很 容易 地 建立 其 运动 学 模型 ; 
je = lcos0 


la = lsin 
若 已 知 关节 变量 9, 则 可 利用 该 模型 直接 求 出 机 械 臂 末端 抓 手 在 参考 系 中 的 位 置 。 
对 于 如 图 4.3 所 示 的 两 自由 度 机 械 臂 ,为 了 建立 其 运动 学 模型 ,同样 需要 建立 一 个 参考 
坐标 系 XOY。 该 机 械 辟 的 关节 空间 有 两 个 变量 0, 和 9, ,工作 空间 有 两 个 变量 和 y。 这 
里 ,为 了 使 机 械 臂 运动 学 模型 具有 一 致 性 ,将 0 和 0 表示 成 带 符号 的 变量 ,因此 对 它们 的 方 
向 做 如 下 定义 : 0 起 始 于 X 轴 指 向 连 杆 1,0, 起 始 于 连 杆 1 的 延长 线 指向 连 杆 2, 同 时 定义 
0, 和 顺 时 针 方 向 为 负 , 逆 时 针 方向 为 正 。 


z (x) 
L 5 


` 
SB、 


图 4.2 单 自由 度 机 械 臂 简 图 图 4.3 两 自由 度 机 械 臂 简 图 
同样 可 以 采用 三 角 几 何 的 方法 建立 该 机 械 臂 的 运动 学 模型 : 
| == L cos0, + Ljcos(0, + 0, ) 


w = Li sin, + l; sin(0; +8) 
到 这 里 ,大 家 是 不 是 觉得 机 器 人 的 运动 学 建 模 很 简单 呢 ? 
接 下 来 ,对 于 如 图 4. 4(a) 所 示 的 6 自由 度 串 联机 器 人 ,能 否 采用 三 角 几 何 的 方法 建立 


| 


(a) 串联 机 器 人 (b) 并 联机 器 人 
图 4.4 6 自由 度 串 联机 器 人 与 3 自由 度 并 联机 器 人 简 图 
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其 运动 学 模型 呢 ? 如 果 可 以 ,那么 对 于 如 图 4. 4(b) 所 示 的 3 自由 度 Delta 并 联机 器 人 ,还 能 
采用 三 角 几 何 的 方法 建立 其 运动 学 模型 吗 ? 答案 基本 上 是 不 可 能 的 。 因 此 ,机 器 人 的 运动 
学 建 模 需 要 系统 专用 的 方法 。 

需要 说 明 的 是 ,如 图 4. 2 和 图 4. 3 所 示 的 机 器 人 ,在 机 构 学 分 类 上 属于 平面 连 杆 机 构 ， 
其 几何 运动 关系 比较 简单 ,所 以 可 以 采用 三 角 几 何 的 方法 建立 其 运动 学 模型 ,而 如 图 4.4 所 
示 的 是 空间 多 杆 机 构 ,其 连 杆 运动 涉及 三 维 空间 的 多 个 象限 ,几何 关系 非常 复杂 ,难以 采用 
平面 三 角 几 何 的 方法 建立 运动 学 模型 。 


4.2.2 D-H 建 模 方法 


1955 年 ,Denavit 和 Hartenberg"" 提 出 了 一 种 基于 齐 次 变换 和 矩阵 的 低 副 机 构建 模 方法 ， 
该 方法 在 每 个 连 杆 上 都 固 连 一 个 坐标 系 , 用 4X4 的 齐 次 变换 矩阵 来 描述 相 邻 两 连 杆 的 空间 
位 姿 关 系 , 通 过 齐 次 变换 建立 机 构 的 运动 学 模型 ,这 种 建 模 方法 被 称 为 Denavit-Hartenberg 
法 ,简称 D-H 法 。D-H 建 模 方法 简单 ,具有 通用 性 ,因此 被 广泛 用 于 机 器 人 的 运动 学 建 模 ， 
至 今 仍 被 广泛 采用 ,在 机 器 人 的 发 展 历程 中 起 到 非常 重要 的 作用 。 

目前 ,D-H 建 模 方法 主要 有 两 种 : 标准 D-H 方法 (Standard D-H method) 和 改进 的 
D-H 方法 (Modified D-H method)。 这 两 种 方法 的 主要 区 别 在 于 连 杆 坐标 系 { 让 建立 的 位 
置 不 同 ,标准 D-H 方法 将 坐标 系 { 沾 建立 在 i 二 1 关节 的 轴线 上 ,而 改进 的 D-H 方法 将 坐标 
系 { 让 建立 在 i 关节 轴线 上 ,当然 这 个 变化 也 带 来 了 D-H 参数 及 D-H 和 矩阵 的 不 同 。Denavit 
和 Hartenberg 提出 的 方法 被 称 为 标准 D-H 方法 ,主要 是 针对 串联 机 构 , 在 处理 树 形 结 构 
(例如 连 杆 末端 连接 两 个 分 支 ) 和 闭 链 结构 的 建 模 时 会 出 现 问题 ,而 改进 的 D-H 方法 则 没有 
这 些 问 题 ,因此 改进 的 D-H 方法 更 具有 通用 性 。 

1. 改进 的 D-H 建 模 方法 

从 机 构 学 的 角度 讲 , 机 器 人 主要 由 关节 和 连 杆 构成 。 如 图 4.5 所 示 的 工业 机 器 人 , 它 是 
H 7 根 连 杆 ( 含 基 座 ) 和 6 个 关节 组 成 的 ,关节 的 转动 导致 了 连接 的 连 杆 的 运动 ,而 各 个 关节 
的 组 合 运动 则 可 实现 预期 的 机 器 人 末端 的 运动 及 位 姿 的 变化 。 下 面 介 绍 如 何 采 用 改进 的 
D-H 建 模 方法 建立 机 器 人 的 运动 学 模型 。 


图 4.5* 6 自由 度 工业 机 器 人 品 
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1) D-H 参数 介绍 . 

在 改进 的 D-H 建 模 方法 中 共 要 使 用 4 个 参数 a,a,d,0, 下 面 逐 一 介绍 。 

(1) 连 杆 i 一 1 的 长 度 aii 

如 图 4.6 所 示 的 是 相 邻 的 两 个 串联 关节 和 连 杆 的 示意 图 : 关节 i 一 1 和 关节 i, 连 杆 i 一 1 
和 连 杆 三 1, 它 们 可 以 是 如 图 4.5 所 示 的 机 器 人 中 的 任意 两 个 相 邻 的 关节 和 连 杆 。 关 节 
0 和 连 杆 0 指 的 是 基 座 。 

在 实际 的 机 器 人 中 , 连 杆 的 长 度 一 般 不 为 零 。 长 度 不 为 零 的 连 杆 的 机 械 功 能 是 连接 两 
个 关节 , 它 的 运动 学 功能 是 保持 与 连 杆 两 端 固 连 的 关节 之 间 固 定 的 几何 位 姿 关 系 。 这 里 连 
杆 的 长 度 并 不 是 其 几何 意义 上 的 长 度 ,而 是 其 运动 学 意义 上 的 长 度 。 

第 2 章 介 绍 过 ,机 器 人 中 的 关节 主要 是 移动 关节 和 转动 关节 。 要 确定 连 杆 的 长 度 首 先 
需要 确定 两 个 关节 的 轴线 。 移 动 关节 的 轴线 就 是 关节 运动 所 在 的 直线 ,转动 关节 的 轴线 由 
右手 定 则 来 确定 : 四 指 弯 曲 方向 与 关节 转动 方向 一 致 ,拇指 所 在 的 直线 就 是 旋转 关节 的 关 
节 轴 线 , 如 图 4.7 所 示 。 


图 4.6 相 邻 关节 与 关节 关系 图 4.7 右手 定 则 


连 杆 i 一 1 的 长 度 ww -定义 为 关节 i—1 的 轴线 和 关节 ;的 轴线 之 间 的 公法 线 长 度 , 它 实 
际 表示 的 是 两 关节 轴线 之 间 的 空间 最 短 距离 。 当 两 关节 轴线 相交 时 , 连 杆 长 度 ai= 0。 

图 4. 6 中 将 连 杆 画 成 弯曲 的 形状 ,目的 就 是 为 了 说 明 D-H 建 模 中 连 杆 长 度 与 连 杆 的 几 
何 形状 是 无 关 的 。 

(2) 连 杆 i 一 1 的 扭 角 w 

连 杆 i 一 1 的 扭 角 w -定义 为 关节 i 一 1 轴线 和 关节 i 轴线 之 间 的 夹 角 , 指 向 为 从 轴线 
i 一 1 到 轴线 i, 如 图 4.6 所 示 。 具 体 的 测量 方法 是 : 以 关节 i 一 1 轴线 与 公法 线 的 交点 为 起 
点 , 作 关 节 i 轴线 的 平行 线 , 则 该 平行 线 就 与 关节 i 一 1 轴线 位 于 同一 个 平面 内 (公法 线 的 法 
平面 ), 这 样 就 可 以 测量 它们 之 间 的 夹 角 。 扭 角 a-1 实 际 表示 的 是 关节 i 的 轴线 相对 于 关节 
i 一 1 的 轴线 的 旋转 角度 (在 法 平面 内 测量 ) ,指向 从 i 一 1 到 i。 

当 两 关节 轴线 平行 时 , 扭 角 a - 5 0。 

例 1: 图 4.8 是 两 个 关节 和 连接 它们 的 连 杆 的 示意 图 ,每 个 关节 都 是 旋转 关节 ,关节 旋 
转轴 如 图 4.8 中 虚线 所 示 。 关 节 和 连 杆 的 连接 关系 有 两 种 ,分别 如 图 4. 8(a) 、4. 8(b) 所 示 ， 
求 两 种 情况 下 的 连 杆 长 度 a: MHH aiio 

解 : 

对 于 连接 方式 1; a;_1= 200,a;-1= 0°; 
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ES SERENE N. Thasa 
© 200 © len © y 


200 


(a) 连接 方式 1 (b) 连接 方式 2 
图 4.8 关节 连 杆 示 意图 


对 于 连接 方式 2: ar- = 200,a;_1 二 90°, 
通过 该 例题 的 练习 ,大 家 可 深入 体会 连 杆 长 度 和 扭 角 的 含义 及 作用 。 连 杆 长 度 和 扭 角 
实际 是 表示 了 两 个 相 邻 关节 之 间 的 距离 和 角度 关系 。 
(3) 连 杆 i 相对 于 连 杆 i 一 1 的 偏 置 d, 
连 杆 i 相对 于 连 杆 i 一 1 的 偏 置 d, 定义 为 : 关节 i 上 的 两 条 公法 线 w 与 a;_1 之 间 的 距离 ， 
沿 关 节 轴 线 i 测量 ,指向 从 a E a;, 如 图 4.9 所 示 。 如 果 关 节 是 移动 关节 , 则 它 是 关节 变量 。 
i 关节 


连 杆 二 1 


六 1 关节 
连 杆 i 计 1 关 节 


图 4.9 相 邻 连 杆 与 连 杆 关系 


(4) 关节 角 0, 

关节 角 0 定义 为 : 连 杆 i 相对 于 连 杆 i 一 1 绕 轴 线 i 的 旋转 角度 , 绕 关 节 轴 线 i 测量 , 指 
向 从 a;- 1 到 a;, 如 图 4.9 所 示 。 如 果 关 节 i 是 转动 关节 , 则 0, 是 关节 变量 。 

实际 测量 方法 如 下 : 过 公法 线 a; -1 与 关节 i 轴线 的 交点 , 作 公 法 线 a, 的 平行 线 , 则 该 平 
行 线 与 公法 线 w -1 的 延长 线 构成 关节 i 轴线 的 法 平面 ,关节 角 0, 在 此 平面 内 测量 ,指向 从 
a;-1 的 延长 线 到 a: 的 平行 线 。 

前 面 介绍 的 4 个 D-H 参数 中 ,前 2 个 参数 描述 连 杆 本 身 , 后 2 个 参数 描述 与 相 邻 连 杆 
的 位 姿 关 系 。 对 于 旋转 关节 ,0; 是 关节 变量 ,其 他 3 个 参数 固定 不 变 , 为 结构 参数 ; 对 于 移 
动 关 节 ,d; 是 关节 变量 ,其 他 3 个 参数 为 结构 参数 。 结 构 参 数 是 由 机 器 人 系统 本 身 的 结构 
确定 的 ,其 中 不 包含 活动 构件 , 当 机 械 结构 装配 完成 后 ,结构 参数 就 确定 了 ,不 会 在 机 器 人 运 
动 的 过 程 中 发 生 改 变 。 

2) D-H 坐标 系 的 建立 

D-H 建 模 方法 需要 在 每 个 连 杆 上 固 连 一 个 坐标 系 , 从 而 利用 连 杆 坐 标 系 描述 两 相 邻 
连 杆 之 间 的 相对 运动 和 位 姿 关 系 。 对 于 一 个 机 器 人 来 讲 ,假设 其 有 nn 个 关节 , 则 需要 建立 


78 l| 机 器 人 学 一 一 机 构 、 运 动 学 、 动 力学 及 运动 规划 


n 十 1 个 连 杆 坐标 系 , 包 括 基 坐 标 系 {0} ,中 间 连 杆 坐 标 系 { 沾 ,末端 连 杆 坐标 系 {n)}。 在 D-H 
建 模 方法 中 ,这 些 连 杆 坐标 系 的 建立 需要 依据 D-H 建 模 方法 的 建 采 规则 ,不 能 随意 建立 , 因 
此 这 些 连 杆 坐标 系 也 被 称 为 D-H 坐标 系 。 

在 建立 D-H 坐标 系 时 ,遵循 的 原则 是 先 建立 中 间 连 杆 坐 标 系 , 再 建立 两 端 连 杆 坐标 系 。 

(1) 中 间 连 杆 坐 标 系 { 沾 的 建立 规则 

以 图 4. 10 所 示 的 关节 连 杆 为 例 , 中 间 连 杆 坐 标 系 的 建立 规则 如 下 : 

MEERA (D KI Z 轴 与 关节 i 的 轴线 共 线 ,其 指向 由 右手 定 则 确定 (如 图 4.7 所 示 ) , 拇 
指 所 指 的 方向 为 正 向 ; 坐标 系 {让 的 X 轴 与 关节 i 和 关节 i 十 1 的 公 垂 线 重合 ,指向 从 i 到 
i 十 1, 当 两 关节 轴线 相交 时 ,X; 王 土 Z;;1 XZ, Y 轴 可 以 依据 右手 法 则 确定 ; 坐标 系 { 让 的 原 
点 O, RAX: AZ 的 交点 ; `4 Z, 和 2 相交 时 ,其 交点 为 坐标 系 { 革 的 原点 ,如 图 4.11(ay) 
所 示 ; 当 Z, 和 2 平行 时 ,坐标 系 { 革 的 原点 取 在 使 偏 置 d, 为 零 处 ,即使 X, AX HR, 
如 图 4. 11(b) 所 示 。 


计 1 关 节 


i 关节 连 杆 i 
连 杆 +1 ”2 关节 


Z Z, Zis 
l | | 
i | | 
Oi 0 
EE s= | X I Xa 
O; Zi I 1 
(a) 如 和 Zi 相交 (b) 加 和 Zi 平行 


图 4.11 坐标 原点 的 确定 


依据 该 规则 ,可 建立 如 图 4. 10 所 示 的 连 杆 坐标 系 {让 和 {i 十 1}。 

(2) 坐标 系 {0} 和 坐标 系 {n} 的 建立 规则 

坐标 系 {0} 的 建立 规则 : 坐标 系 {0} 的 建立 要 参考 坐标 系 {1)。 选 择 坐 标 系 {0} 的 Z. 
X 轴 与 坐标 系 {1} 的 Z、X 轴 同 向 ,如 果 可 能 ,使 坐标 系 {0} 与 坐标 系 {1} 重 合 。 其 目的 是 使 尽 
可 能 多 的 D-H 参数 为 零 , 从 而 简化 后 续 的 矩阵 计算 。 

坐标 系 {n} 的 建立 规则 : 

坐标 系 {n} 的 Z 轴 要 根据 关节 ? 的 运动 由 右手 定 则 确定 ,选择 坐标 系 {n} 的 X 轴 与 坐标 
系 {2 一 1} 的 X 轴 同 向 。 

坐标 系 {0} 与 坐标 系 {z} 的 原点 需 结 合 机 器 人 的 结构 及 运动 学 建 模 需求 来 确定 。 
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(3) D-H 坐标 系 建立 步骤 总 结 

D-H 坐标 系 的 建立 原则 是 先 建立 中 间 坐 标 系 (i) ,再 建立 两 端 坐 标 系 {10} 和 {nn}。D-H 
坐标 系 的 建立 步骤 如 下 : 

O 确定 Z BB: 根据 关节 ; 的 轴线 及 关节 转向 采用 右手 定 则 确定 Z, 轴 。 

@ 确定 原点 O: 如 果 两 相 邻 关节 轴线 Z, 与 Z;41 不 相交 , 则 公 垂 线 与 乙 轴 (轴线 六 的 交 
点 为 原点 ; 当 Z, 与 Zi+1 平 行 时 ,原点 的 选择 应 使 偏 置 d;4 1 为 零 ; 如 果 Z, 与 Z, 相交 则 交 
点 为 原点 ; WR Z 与 Zi+1 重 合 则 原点 应 使 偏 置 di41 为 零 。 

© 确定 X; 轴 : 两 轴线 不 相交 时 ,X; 轴 与 轴线 Z, 与 Z;41 的 公 垂 线 重合 ,指向 从 i 到 i 十 
1; 车 两 轴线 相交 , 则 X; 轴 是 Z, 与 2 两 轴线 所 成 平面 的 法 线 X = EZ X 2;; 如 果 两 轴 
线 重合 , 则 X; 轴 与 Z; 及 Zi41 两 轴线 垂直 且 使 其 他 连 杆 参数 尽 可 能 为 零 。 

D 按 右手 定 则 确定 YA. 

@ 坐标 系 {0} 可 任意 建立 ,一 般 选 择 坐 标 系 {0 } 与 坐标 系 {1} 重 合 。 

坐标 系 {n} 的 Z 轴 由 关节 ?7 的 运动 类 型 和 右手 定 则 确定 ,原点 O, 与 X, 轴 可 任意 选 
择 , 但 一 般 选择 X, 轴 与 坐标 系 {n 一 1) 的 XX 轴 同 向 ,从 而 使 尽 可 能 多 的 D-H 参数 为 零 。 

3) 利用 连 杆 坐标 系 确定 D-H 参数 

在 D-H 建 模 方法 中 ,建立 了 机 器 人 的 D-H 连 杆 坐标 系 后 ,可 直接 从 相 邻 的 两 个 连 杆 坐标 
系 确定 D-H 参数 。 对 于 图 4. 12 所 示 的 连 杆 坐标 系 {i 一 1) 和 {让 ,其 D-H 参数 的 确定 方法 如 下 : 

aii: 从 Zi-1 到 2; 沿 X;- 1 测量 的 距离 ; 

ai: 从 ZB Z, 绕 X;-1 旋 转 的 角度 ，; 

di: 从 X;_ X; Wr Z, 测量 的 距离 ; 

0;: A XF X 2 Z, 旋转 的 角度 。 


天 1] 关节 tfii 
连 杆 i mkt 


图 4.12 连 杆 坐标 系 与 D-H 参数 


4) D-H 矩阵 

建立 了 D-H 坐标 系 , 也 确定 了 相 邻 连 杆 的 D-H 参数 ,那么 ,如 何 表示 两 相 邻 连 杆 之 间 
的 位 姿 关 系 呢 ? 这 就 要 用 到 第 3 章 中 介绍 的 坐标 变换 知识 。 

如 图 4.12 所 示 ,对 于 坐标 系 4 人 一 1} 和 人 4) ,坐标 系 {i 一 1} 经 过 下 列 四 次 运动 变换 到 坐标 
# (i): a 

D AE PR28411) Kiz 1 转动 Qa;-1 :使 pe” 与 Zi 同 问 ; 

© 坐标 系 人 一 1) 沿 X,-, 移动 a; 1; 使 Zi i 与 Z, 共 线 ; 
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© 坐标 系 {i 一 1}) 绕 Z, 转动 0 ,使 X, X: 同 向 ; 
@ 坐标 系 {i 一 1) 沿 Z 移动 d ,使 X;-1 与 X, 共 线 ,坐标 系 (i=1) 与 {四 共 原 点 。 
上 述 坐 标 系 变 换 中 ,坐标 系 {i 一 1} 都 是 相对 于 自己 的 坐标 轴 做 旋转 或 平移 ,因此 所 有 的 
子 变换 都 是 相对 于 动 系 的 ,所 以 坐标 系 { 夺 相对 于 坐标 系 位 一 二 的 转换 矩阵 为 : 
:T= Rot(X,a, )Trans(X ,ai )Rot(Z,0;)Trans(Z,d;) 


cosĝ; 一 sin; 0 ii 
sinĝ;COSa;—ı cosboicosw- — sina; — sina; d; 
1 š ç (4-1) 
sinĝ;sina;—ı cosóñ,sina;_, COSQ;_1 cosa; d; 
0 0 0 1 


该 矩阵 通 式 被 称 作 D-H 矩阵 , 它 是 一 个 4X4 的 齐 次 变换 矩阵 ,采用 4 个 D-H 人 参数 表 
示 相 邻 两 连 杆 之 间 的 位 姿 关 系 或 转换 关系 。 

5) 机 器 人 的 运动 学 建 模 

对 于 具有 nn 个 自由 度 的 机 器 人 ,在 建立 其 D-H 坐标 系 并 确定 相 邻 坐标 系 之 间 的 D-H 
参数 后 , 即 可 获得 n 个 如 式 (4-1) 所 示 的 D-H 矩阵 ,将 所 有 和 矩阵 按 顺 序 连 乘 , 即 可 得 到 该 机 
器 人 的 运动 学 模型 ,6 自由 度 机 器 人 的 运动 学 模型 如 下 : 

oT= T -iT TT .iT -ST 
=i TC) ATC) ATC) +3 T(q ) ITC) ATC) 


ÎR $P 
u P 1 l 
在 机 器 人 的 运动 学 模型 中 ,各 个 关节 变量 就 是 该 运动 学 方程 的 变量 。 如 果 确 定 了 各 关 
节 变 量 , 则 可 唯一 确定 机 器 人 末端 连 杆 坐标 系 {z 在 基 坐 标 系 {0} 中 的 位 姿 。 
6) D-H 运动 学 建 模 习 题 
例 2: 图 4.13 所 示 是 一 个 3 自由 度 机 器 人 的 示意 图 ,三 个 关节 皆 是 旋转 关节 ,第 3 关节 
轴线 垂直 于 1、2 关节 轴线 所 在 的 平面 。 各 个 关节 的 旋转 方向 如 图 中 所 示 。 要 求 按 改 进 的 
D-H 方法 建立 各 连 杆 坐标 系 ,并 建立 D-H 参数 表 , 求 出 该 机 器 人 的 D-H 运动 学 模型 。 


| 


图 4.13 3 自由 度 机 器 人 示意 图 


解 1: 

(1) 首先 ,依据 D-H 坐标 系 的 建立 方法 建立 所 有 的 连 杆 坐标 系 , 如 图 4. 14 所 示 。 坐 标 
系 {1} 和 {2} 的 Z、X 轴 很 容易 确定 ,所 以 坐标 系 的 建立 比较 容易 。 et } 时 ,由 于 
只 有 Z, 的 方向 是 确定 的 ,所 以 将 X, 选 在 使 偏 置 d, 为 零 的 位 置 。 由 于 坐标 系 {4} 只 是 用 来 
表示 末端 的 位 姿 , 所 以 只 有 坐标 原点 是 确定 的 ,这 里 将 其 各 坐标 轴 与 坐标 系 {3 Ea 
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4.14 3 自由 度 机 器 人 D-H 坐标 系 的 建立 


(2) 根据 图 4. 14 建立 的 连 杆 坐标 系 , 读 取 所 有 相 邻 连 杆 坐标 系 的 D-H 参数 ,并 建立 
D-H 参数 表 , 如 表 4.1 所 示 。 注 意 ,各 关节 变量 的 初 值 ( 即 当 前 值 ) 也 需要 表示 出 来 。 


表 4.1 D-H 参数 表 


(3) 将 各 行 D-H 参数 代入 D-H 矩阵 通 式 (4-1) ,可 得 4 个 D-H WE: 


[cos0, w sin0, 0 0 
Pe sin0, cos0, 6 Ó 
2 0 0 1 0 
| 0 0 0 1 
[cosO — sinĝs 0 an 
T= sinf; cos, O Q 
0 0 1 0 
0 0 0 1 
cosh; —sing 0 a, 
0 0 1 O 
=| ` 
一 sin0 一 cos0 0 0 
0 0 0 1 
T O Q Q 
i 0 1 © 0 
T= 
0 QL d 
O © 1° 
(4) 将 4 个 D-H 和 矩阵 连 乘 起 来 , 即 可 得 到 该 机 器 人 的 运动 学 模型 : 


. I R P 
ITT ATAT AT TG T= |. Re A 
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[cos(0, + 0,)cos0, — cos(0 + 0z ) sinô; = sln (0, Fh 
iR = |sin(0, + 0,)cos@ó; —sin(0 十 0)sing cos(0, + 0;) | 
h: — sin0, — cosh; 0 

[aocosb + aicos(0, + 0,) — dsin(0, + 02) 
(P = |assin0, Ha sinh + 0,) + dceos(0, + 0; ) 


0 


(5) 验证 模型 的 正确 性 。 
假设 该 机 器 人 的 结构 常数 为 mw 王 10,w 王 20,d=30, 从 表 4.1 中 可 得 到 各 关节 变量 的 初 值 
为 0 二 二 二 0, 将 结构 常数 及 各 关节 变量 的 初 值 代入 到 机 器 人 的 运动 学 模型 中 ,可 得 : 
1 0 © 30 


4 lp2 papa 0 0 1 30 
oT = TIT; TT = TES a 
0 Q Q 1 


上 述 得 到 的 齐 次 矩阵 就 是 末端 坐标 系 {4} 在 基 坐 标 系 1{0} 中 的 位 姿 。 将 该 矩阵 表示 的 位 
姿 与 图 4. 13 中 坐标 系 {4} 相 对 于 坐标 系 {0} 的 位 姿 进 行 对 照 ,如 图 4.15 所 示 ,很 显然 两 者 相 
等 ,因此 可 证 明 所 建立 的 D-H 运动 学 模型 的 正确 性 。 

解 2: 

对 于 图 4. 13 所 示 3 自由 度 机 器 人 ,解答 1 按照 改进 的 D-H 建 模 方法 建立 了 4 个 坐标 系 ， 
而 第 4 个 坐标 系 纯粹 是 为 表示 机 器 人 末端 的 位 姿 而 建立 的 , 它 与 第 3 个 坐标 系 之 间 是 纯 平移 
关系 。 在 实际 应 用 中 ,通常 省 略 掉 第 3 个 坐标 系 , 而 直接 在 机 器 人 的 末端 建立 一 个 坐标 系 {3)， 
如 图 4.16 所 示 。 这 样 会 省 掉 一 个 坐标 系 的 建立 ,并 在 运动 学 建 模 时 减少 一 组 矩阵 乘法 运算 。 


图 4.15 齐 次 矩阵 与 坐标 系 、 图 4.16 3 自由 度 机 器 人 D-H 坐标 系 建立 
位 姿 的 对 应 关系 


基于 图 4. 16 建立 的 各 坐标 系 , 可 得 如 表 4. 2 所 示 的 D-H 参数 表 。 相 对 于 表 4. 1 的 
D-H 参数 表 ,该 表 减 少 了 一 行 。 
表 4.2 D-H 参数 表 2 
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m 


将 表 4.2 中 的 D-H 参数 代入 D-H 和 矩阵 通 式 (4-1) ,可 得 3 个 D-H 和 矩阵 ， 
55 六“ 一 5 O. Q 


i sin0, cos0, ora 
oT s5 
0 0 a >O 
0 0 O : 
cos; —sin0, 0 ao 
T= sinb» cos Q O 
0 0 1 O 
| Ó 0 Gi 
[ cos 一 Sinb 0 a 
I 0 0 1: "g 
zT = ; 
一 Sinb — cosh 0 0 
0 0 0 1 
将 上 述 D-H 和 矩阵 连 乘 , 可 得 机 器 人 的 运动 学 模型 ; 
ae òR 3P 


[cos(0, + 0,)cos@, — cos(0, + 0,;)sin0, — sin(ð, +02) 
3R = |sin(0, + 0,)cos@,;, — sin(0, +@0,)sinó,r cos(0 + 0,) 
L es sinĝ; =s cosh; 0 


[aocosb + aicos(0 + 0,) — dsin(0, to 


3P = |assin0, 十 asin(0 十 2) 十 wwcos( +02) 
L 0 
将 机 器 人 的 结构 常数 及 关节 变量 初 值 aa =10,a,=20,d,=30,0, =0,=0,=0 代入 到 机 
器 人 的 运动 学 模型 ,可 得 : 


1 0 0 30 
0 0'E 30 
00 =1 0 0 
0 a 0 1 
对 照 该 矩阵 与 图 4. 16 所 示 的 坐标 系 {3} 相 对 于 坐标 
系 {0} 的 位 姿 关 系 ,可 知 该 机 器 人 的 运动 学 模型 是 正确 的 。 
由 于 图 4. 16 中 的 坐标 系 {3) 与 图 4.14 中 的 坐标 系 {4} 位 姿 
一 致 ,因此 两 种 方法 求 得 的 末端 位 姿 矩 阵 一 样 , 即 ?T==%7T， 
上 述 计算 结果 也 证 明了 这 一 点 。 
例 3: 图 4.17 是 一 个 3 自由 度 机 器 人 的 示意 图 ,3 个 
关节 皆 是 旋转 关节 ,第 2 关节 轴线 垂直 于 1.3 关节 轴线 。 
各 个 关节 的 旋转 方向 如 图 4.17 所 示 。 要 求 按 改进 的 D-H 
方法 建立 各 连 杆 坐标 系 , 建 立 D-H 参数 表 , 并 求 出 该 机 器 
人 的 D-H 运动 学 模型 。 j 


图 4.17 3 自由 度 机 器 人 示意 图 
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解 1: 

(1) 首先 ,依据 D-H 坐标 系 的 建立 方法 建 
立 所 有 的 连 杆 坐标 系 , 如 图 4. 18 所 示 。 由 于 
Z, 与 Z, 的 交点 位 于 关节 2 上 ,所 以 坐标 系 {1) 
需要 建 在 关节 2 E, H X 要 垂直 于 Z, 5 Z; 
构成 的 平面 ,Xi 有 两 个 方向 可 选 ,这 里 选择 朝 
外 的 方向 。 而 {0} 坐 标 系 要 建立 在 基 座 上 ,所 
以 在 这 个 例子 中 ,{0} 坐 标 系 与 11} 坐 标 系 必须 
是 分 开 的 ,这 里 将 {0} 坐 标 系 的 姿态 与 11} 坐 标 
系 的 一 致 。 坐 标 系 {2) 的 情况 与 坐标 系 {1}) 类 
WAT D-H 参数 中 零 值 尽 可 能 多 ,选择 X, 
与 X, 同 向 。 坐 标 系 {3} 建 立 在 机 器 人 的 末端 ， 
选择 X, 与 X, 同 商 。 图 4.18 3 自由 度 机 器 人 的 D-H 坐标 系 

(2) 根据 建立 的 连 杆 坐 标 系 , 读 取 所 有 相 邻 连 杆 坐标 系 的 D-H 参数 ,并 建立 D-H 参数 
表 , 如 表 4. 3 所 示 。 


表 4.3 D-H 参数 表 3 


(3) 将 各 行 D-H 参数 代入 D-H 和 矩阵 通 式 (4-1) ,可 得 三 个 D-H 矩阵: 


[cos@, kaa sin0, 0 0 
sin0, cos, Q Q 
0 0 1 dı 
l. O 0 © 1 
[ cosh, — sin Ü O 
0 0 L OÚ 
Ir = 
—sin0; — cosh 0 0 
EL 0 O l 
[cos0; > sinĝ; 0 0 
0 0 == — az 
T= 
sin0, cosh; 0 0 
to 0 0 1 
(4) 将 三 个 D- 互 抢 阵 连 乘 起 来 , 即 可 得 到 该 机 器 人 的 运动 学 模型 ; 
hoi l iR iP 
T =T T T= iTr T TY = | ] 
ag 1 
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(5) 验证 。 
将 机 器 人 的 结构 常数 及 关节 变量 初 值 ai 二 10,a; = 二 20,0 =0,=80,=0 代入 到 机 器 人 的 
运动 学 模型 ,可 得 : 


oT 一 0T1T3T = 


O O me O 
O = oO oO 
° 


l 
0 
0 
0 

对 照 该 矩阵 与 图 4. 18 所 示 的 坐标 系 {3} 相 对 于 
坐标 系 10} 的 位 姿 关 系 , 可 知 该 机 器 人 的 运动 学 模型 
是 正确 的 。 

解 2: 

在 解 1 中 ,坐标 系 10} 的 建立 是 选择 与 坐标 系 
{1} 的 姿态 相同 。 这 里 有 个 问题 : 坐标 系 {0} 的 姿态 
是 否 可 以 任意 选 定 ? 

为 了 回答 这 个 问题 ,建立 {0} 坐 标 系 ,如 图 4. 19 
所 示 。 依 据 建立 的 坐标 系 可 得 该 机 器 人 的 D-H # 


数 表 如 表 4.4 所 示 ,该 参数 表 与 表 4. 3 最 大 的 不 同 
E 0, 的 初 值 变 了 。 图 4.19 3 自由 度 机 器 人 的 D-H 坐标 系 


表 4.4 D-H 参 数 表 4 


co li li 一 | ~ 


将 各 D-H 矩阵 连 乘 得 到 机 器 人 的 运动 学 模型 ,并 将 结构 常量 和 关节 变量 初 值 w — 10, 
a, =20,0, = —90°,0, =0, =0° 代入 到 机 器 人 的 运动 学 模型 中 ,得 : 


o | Š y 
; =I Ü Ü W 
iT =iTiT}T = 
0- Ó 1 30 
0 0 0 1 
将 该 矩阵 与 图 4. 19 表示 的 坐标 系 {3} 相 对 于 坐标 系 {0} 的 位 姿 关 系 进行 对 照 ,可 知 该 机 
器 人 的 运动 学 模型 是 正确 的 。 


结论 : 对 于 基 坐 标 系 10} ,在 没有 其 他 约束 条 件 时 ,可 按照 需要 建立 笛 卡 儿 坐 标 系 , 而 不 
必 让 坐标 系 {0) 与 坐标 系 {1) 的 姿态 相同 。 

需要 说 明 的 是 ,坐标 系 {0} 的 原点 是 由 基 座 的 位 置 决定 的 。 男 外 , 例 3 中 所 讨论 的 机 器 
人 的 结构 通常 是 6 自由 度 工业 机 械 臂 的 后 3 个 关节 的 构 型 。 

例 4: 图 4.20 是 一 个 3 自由 度 机 器 人 的 示意 图 ,3 个 关节 皆 是 旋转 关节 ,第 2 关节 相对 
于 第 1 关节 有 偏 置 。 各 个 关节 的 旋转 方向 如 图 4. 20 所 示 。 要 求 按 改进 的 D-H 法 建立 各 连 
杆 坐 标 系 ,建立 D-H 参数 表 , 并 建立 该 机 器 人 的 D-H 运动 学 模型 。 
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解 : š 

同样 ,依据 D-H 坐标 系 的 建立 方法 建立 所 有 的 连 杆 坐标 系 ,如 图 4. 21 所 示 。 由 于 2 
与 Z, 的 交点 位 于 水 平 连 杆 上 ,所 以 坐标 系 {1) 需 要 建 在 Z, 与 Z, 的 交点 处 , 且 X, 要 垂直 于 
Z, 与 Z, 构成 的 平面 ,Xi 选择 朝 外 的 方向 。 坐 标 系 {0} 要 建立 在 基 座 上 ,选择 坐标 系 10) 的 
姿态 与 坐标 系 {1} 的 一 致 。 坐 标 系 {2) 的 情况 与 坐标 系 {1} 类 似 ,选择 X, 与 X, 同 向 。 坐 标 
系 {3} 建 立 在 机 器 人 的 末端 ,选择 X, 与 X, 同 向 。 


图 4.20 3 自由 度 机 器 人 示意 图 图 4.21 3 自由 度 机 器 人 的 D-H 坐标 系 


根据 建立 的 D-H 坐标 系 , 读 取 D-H 参数 ,建立 D-H 参数 表 , 如 表 4.5 所 示 。 在 读 取 
D-H 参数 时 ,要 用 右手 定 则 确定 扭 角 的 正 负 号 。 


表 4.5 D-H 参数 表 5 


由 各 行 D-H 参数 ,可 得 D-H 和 矩阵 如 下 : 


| cosb， 一 Sin 


0 0 
T= sin0, cos), 0 : 
0 0 1 an 
0 
0 
d 
0 
i] 


0 0 


| cos: — sin0, 


0 
0 0 1 
—sin0, —cos0, 0 
0 


3 
| 


0 0 
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cOs0 — sinb 0 0 
r 0 0 =f = äi 
iT = 
sinĝ; cosh; 0 0 
0 0 0 j! 
将 各 D-H 矩阵 连 乘 可 得 机 器 人 的 运动 学 模型 ; 
I ! , | oR aP 
aT =i TT IT =¿T(0, ATO Ts | | 
Q Qg T 


将 结构 常数 和 关节 变量 初 值 ao =10, a; =20, d =30 , 0, 三 多 二 多 二 0 代入 机 器 人 的 运动 


学 模型 ,可 得 : 


tL O Q 0 

r> 0 1 O 30 
PETT = 

0 0O 1 30 

O Q Ü 1 


经 验证 ,该 矩阵 表达 的 坐标 系 {3} 相 对 于 坐标 系 40} 的 位 姿 关 系 与 图 4. 21 中 两 坐标 系 的 
位 姿 关 系 一 致 ,因此 该 运动 学 模型 正确 。 

2. 标准 D-H 建 模 方法 

标准 D-H 建 模 方 法 与 改进 D-H 建 模 方 法 的 最 大 区 别 是 坐标 系 { 引 的 位 置 不 同 , 随 之 带 
来 的 是 D-H 参数 的 不 同 。 下 面 以 图 4. 22 为 例 对 标准 D-H 建 模 方法 做 简要 介绍 。 


i 关节 


图 4.22 标准 D-H 建 模 方法 


标准 D-H 建 模 方法 的 坐标 系 建立 原则 如 下 : 

@ 坐标 系 人 让 的 Z 轴 与 关节 i 十 1 的 轴线 重合 ; 

© 坐标 系 他 的 坐标 原点 设 在 关节 i 和 关节 i 十 1 的 轴线 公 垂 线 与 关节 i 十 1 的 轴线 交点 处 ; 
© 坐标 轴 X; 与 关节 i 和 关节 i 十 1 的 轴线 公 垂 线 重合 ,从 关节 i 指向 关节 i 十 1。 

标准 D-H 建 模 方法 的 D-H 参数 值 规 定 如 下 : 

O di H Za MA XAA X: 的 距离 ,与 Z;_1 方 向 相同 为 正 ; 

© 0, WAZAMA Xal X, 的 转角 , 逆 时 针 方向 为 正 ; 

O a, 为 沿 X, 从 Zi 到 2 的 距离 ,与 X, 方向 相同 为 正 ; 
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@ ui 为 绕 X; 从 Zi;-1 到 2; 的 转角 , 逆 时 针 方 向 为 正 。 

如 图 4. 22 所 示 ,坐标 系 {i 一 1}) 变 换 到 坐标 系 { 引 可 分 为 下 列 4 个 子 变换 : 

O 坐标 系 {i 一 1} 绕 Z;-1 转 动 9, 使 X;-1 与 X, 同 向 ; 

© 坐标 系 {i 一 1}) 沿 Zi- 1 移动 d; ,使 Xi-1 与 X; 共 线 ; 

© 坐标 系 (i 一 1} 沿 X, 移动 a; ,使 坐标 系 (i 一 1} 与 坐标 系 { 让 共 原 点 ; 

@ 坐标 系 人 {i 一 1) 绕 X, 转动 w ,使 2Z;_1 与 Zi; 共 线 。 

所 有 的 变换 过 程 都 是 相对 于 运动 坐标 系 的 坐标 轴 , 所 以 可 得 坐标 系 { 引 相对 于 坐标 系 (i 一 
1) 的 位 姿 矩 阵 为 : 

:1T=*Rot(Z,0;)Trans(0,0,d;)Trans(a;,0,0)Rot(X ,a;) 


ch; — sO0ica; sOisa; a;c0,; 


s, cca, 一 cbosai aish; 

(4-2) 
0 sa; CQi d, 
0 0 0 1 


例 5: 对 于 图 4.13 所 示 的 3 自由 度 机 器 人 ,采用 标准 D-H 建 模 方法 建立 D-H 坐标 系 
和 D-H 参数 表 , 求 其 运动 学 模型 。 

解 : 

建立 的 D-H 坐标 系 如 图 4. 23 所 示 ,建立 的 D-H 参数 表 如 表 4.6 所 示 。 


图 4.23 标准 D-H 建 模 方法 建立 坐标 系 


表 4.6 D-H 参数 表 6 


将 上 述 D-H 参数 表 的 各 行 代入 到 D-H 和 矩阵 通 式 (4-2) 中 ,可 得 各 D-H 和 矩阵 如 下 : 


[cos —sin0, 0 an 
; sin0, cos, Ü 0 
oT = 
0 0 1 O 
L 0 0 0 1 
Ñ cos0, = sin0, ali 
0 0 0 


0 
z 1 
é T 一 
i - sing, —cos0, 0 0 
0 0 DA 
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R xP 
g Ó 1 | 
— sin0, 

cos0, 


0 


cosh — sin0, 0. 07 
T= siny cosh L O = 0 
0 0 1. a 
0 0 Ó l| 
将 各 D-H 矩阵 顺序 连 乘 , 可 得 机 器 人 的 运动 学 模型 为 : 
ST =1T TT =1T(0,) FTO) STO) = f: 
rcosQicos(0, F0) = cos@,sin(80, +0) 
iR = |sin0,cos(0, +0) — sin0,sin(0, + 6, ) 
L — sin(0, + 0.) 一 cos(b + 0.) 
[au + ai cosb 一 dsin0, 
iPS a sinf, + dcos0, 
L 0 
将 结构 常量 和 关节 变量 初 值 ww 王 10,a =20, d =30,0, 二 ,二 二 0" 代 入 到 机 器 人 的 运 
动 学 模型 ,可 得 : 
1] 0 0 30 
(T = Ti TT = S a a 
 9Ú0 3 0 g 
0 0 0 1 
) 


将 该 矩阵 与 图 4. 23 中 坐标 系 {3} 相 对 于 坐标 系 {0} 的 位 姿 进 行 对 比 ,两 者 一 致 ,从 而 证 


明了 上 述 D-H 建 模 过 程 的 正确 性 。 


从 前 面 介绍 的 标准 D-H 建 模 方法 与 改进 的 D-H 建 模 方法 来 看 ,两 者 都 可 以 建立 机 器 


人 的 运动 学 模型 ,只 不 过 是 坐标 系 的 建立 规则 和 参数 定义 不 同 


。 由 于 改进 的 D-H 建 模 方 法 


将 坐标 系 {让 建立 在 i 关节 上 ,理解 起 来 更 容易 些 。 而 标准 D-H 建 模 方法 中 ,坐标 系 {z} 建 立 


在 ;十 1 关节 上 ,因此 在 初学 时 会 有 些 困 扰 。 


4.3 运动 学 逆 解 方法 


已 知 机 器 人 的 关节 角 ,利用 机 器 人 的 运动 学 模型 可 以 计算 出 机 器 人 的 末端 坐标 系 相 对 


于 基 坐 标 系 的 位 姿 ,但 在 机 器 人 的 实际 应 用 中 ,通常 机 器 
人 的 运动 轨迹 是 确定 的 ` 已 知 的 ,为 了 控制 机 器 人 实现 预 
定 的 轨迹 ,需要 从 机 器 人 的 运动 轨迹 反 解 出 对 应 的 关节 
变量 ,这 被 称 作 运动 学 求 逆 或 求 运动 道 解 。 如 图 4.24 所 
示 , 工 件 上 的 焊 缝 位置 是 确定 的 ,为 了 控制 机 器 人 完成 焊 
接任 务 ,就 需要 采用 运动 学 逆 解 求 出 对 应 的 关节 运动 变 
量 , 然 后 将 该 关节 运动 变量 输入 机 器 人 的 控制 器 ,机 器 人 
就 能 够 带 着 焊 炬 走出 期 望 轨迹 ,完成 焊接 任务 。 

对 于 串联 机 器 人 , 正 运动 学 的 解 是 唯一 的 ,而 逆 运 动 
学 的 解 则 不 唯一 ,对 于 并 联机 器 人 则 正 相 反 。 运 动 学 求 


焊接 机 器 人 


图 4. 24 
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逆 是 机 器 人 运动 规划 与 轨迹 控制 的 基础 ,是 机 器 人 学 中 重要 的 研究 内 容 。 
机 器 人 的 逆 运 动 学 是 已 知 机 器 人 末端 的 位 姿 , 求 对 应 的 关节 变量 ,如 式 (4-3) 所 示 , 其 
中 等 号 右边 齐 次 矩阵 已 知 , 等 号 左边 关节 变量 未 知 。 


n y 
o T(Q sQ s "°" sQ, ) = Tüg, Ty — ` ` Š I (4-3) 
n: z 


式 (4-3) 写 成 标量 形式 如 下 : 
naa = n(q) = n(qi,q2 4s""" ,qn) 
03x1 = 0(9) = 0(9qi gz2 ， 93， sqa) 
asxl = alq) = alqi »q23q33** sqn) 
bsa = p) = 力 (91，g2 9g3， qa) 
简写 为 Prex =f) qR". 
假设 4 二 6, 则 上 述 北 运动 学 方程 有 6 个 未 知 数 , 但 有 12 个 方程 ,而 且 noa 所 关联 的 9 
个 方程 中 只 有 3 个 是 独立 的 ,位 置 p,、p,、p: 关联 的 3 个 方程 是 独立 的 ,因此 只 有 6 个 独立 
方程 和 6 个 未 知 数 ,这 种 方程 组 被 称 为 非 线性 的 超越 方程 组 ,求解 非常 复杂 。 这 就 是 机 器 人 
运动 学 求 逆 困难 的 原因 。 
以 机 器 人 操作 臂 为 例 ,其 逆 运 动 学 解法 主要 有 两 类 : 数值 解 (Numerical Solution) 和 解 
析 解 (Analytical Solution), 


4.3.1 逆 运 动 学 的 数值 解 方法 
所 谓 的 数值 解 就 是 采用 某 种 计算 方法 得 到 式 (4-3) 的 一 组 近似 解 ,能 在 满足 给 定 精度 
的 情况 下 使 式 (4-3) 成 立 。 数 值 解法 只 能 求 出 方程 的 特 解 ,不 能 求 出 所 有 的 解 。 数 值 解法 
主要 有 数值 盘 近 法 、 差 值 法 有限 元 法 等 。 例 如 ,可 用 和 迭代 的 方法 最 小 化 机 器 人 末端 执行 器 
与 目标 点 之 间 的 距离 , 求 出 机 器 人 的 运动 学 逆 解 。 数 值 解法 的 优点 是 计算 简单 ,不 需要 做 和 矩 
阵 转换 ; 缺点 是 迭代 次 数 多 ,实时 性 差 ,不 适合 用 于 实时 性 要 求 高 的 场合 , 且 机 器 人 运动 过 
程 中 的 位 形 不 可 预测 ,不 适合 用 于 障碍 空间 中 机 器 人 的 
运动 解 算 。 
下 面 举例 说 明 如 何 采 用 数值 解法 求 机 器 人 的 逆 解 。 
图 4. 25 所 示 是 一 个 两 自由 度 机 器 人 的 机 构 简 图 ， 
它 有 一 个 旋转 关节 和 一 个 移动 关节 。 
采用 几何 法 可 求 得 机 器 人 的 正 向 运动 学 模型 ; 
图 4.25 两 自由 度 机 器 人 简 图 a 
y = rsin 
将 机 器 人 目标 位 置 与 当前 位 置 的 差 定义 为 向 量 函 数 f(X): 
Susa — COSA 0 
F = Pus Bz. a M 


机 器 人 运动 到 目标 位 置 意 味 着 f(X) 的 模 最 小 ,因此 利用 :7(X) 建 立 目标 函数 : 


+, z Ç 
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1 I [iw rN ,ri = reos0 
FOD = 二 "OD .f00 = 1 | | | | (4-4) 


Z L yasa — rsin0 aga — F'atni) 
这 样 就 将 机 器 人 运动 学 求 逆 问 题 转化 为 求 min F(X) 问 题 。 
FX) 的 一 阶 泰勒 展开 式 为 : 
F(X+kh) = F(X) +F'(X) » (kh) + o(#h) 
其 中 ,h 为 二 维 向 量 , 表 示 X I 3⁄0) Jr] k 为 h 的 系数 。 
于 是 有 : 


lim FOO — FOX + kh) _ _ F'(X)h 
io k || h || Ilh || 


其 中 ,a 是 h AFX) fA. HeT DL. 4 0= x 时 ,F(X) 下 降 最 快 , 即 一 F(X) 是 
F(X) 最 快 下 降 方 向 。 
由 式 (4-4) 得 : 


=— F'(X)cosa 


F(X) = x = 


使 用 最 快 下 降 法 可 得 到 关节 的 位 移 量 [A9 Ar] : 
mi = APX = | rsin0 Hsi Esa = 5 


Ar rcosO  sinĝdL y. a — rsin0 


I ik,u] (8 X HL 68 A BJ 3 iz 23JJ2 F fg BJ RUER aK: 
S 0 Ab 0, —rsin0 cos0 Kari = COSO 
[|= [上 A] LF: : | rcos0 soli i piai ka 
假设 该 机 器 人 末端 从 位 置 (1,1) 移 动 到 (2,3),& 取 值 为 0.01, 精 度 设 定 为 0.01, 则 利用 
迭代 式 (4-5) 经 过 538 次 迭代 , 求 得 了 对 应 的 机 器 人 两 关节 运动 的 解 ,两 关节 的 运动 曲线 如 
图 4. 26(a) 所 示 。 将 求 得 的 两 关节 运动 解 代入 机 器 人 的 正 向 运动 学 模型 ,可 求 得 机 器 人 末 


端的 运动 轨迹 如 图 4.26(b) 所 示 。 从 图 中 可 以 看 出 , 求 得 的 机 器 人 两 关节 的 运动 轨迹 及 机 
器 人 末端 的 运动 轨迹 都 是 光滑 的 ,可 以 满足 机 器 人 的 运动 控制 的 需求 。 


dF _ | rsing — j Ë 党 | 


一 7rcosO 一 Sinb Sagt ~ FSIE. 


1 4 3 
2.8 
0.95 2.6 
š 2.4 
0.9 92 
y £ g 
£ S: 5 = 2 
0.85 1.8 
1 1.6 
1.4 
0.8 T 
1.24 
l 
0750 200 400 600 W 20 400 600 l 1.5 2 
n/ 次 n/ 次 x/m 
(a) 两 关节 的 解 ， (b) 机 器 人 末端 运动 轨迹 


图 4.26 道 运 动 学 反 解 及 机 器 人 末端 运动 轨迹 
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4.3.2 逆 运 动 学 的 解析 解 方法 š 

机 器 人 逆 运 动 学 的 解析 解 通常 被 称 为 封闭 解 (Closed Form Solution) ,就 是 可 以 将 求解 
的 关节 变量 表示 成 解析 表达 式 的 形式 ,是 式 (4-3) 的 精确 解 ,能 在 任意 精度 下 使 式 (4-3) 等 式 
成 立 , 具 有 计算 速度 快 ,便于 实时 控制 等 优点 。 

Donald Lee Pieper AWRA: 如 果 串 联机 器 人 操作 臂 在 结构 上 满足 下 面 两 个 充分 
条 件 中 的 一 个 ,就 会 有 解析 解 。 这 两 个 充分 条 件 也 被 称 为 Pieper 准则 , 即 : 

(1) 三 个 相 邻 关节 轴线 交 于 一 点 ; 

(2) 三 个 相 邻 关 节 轴 线 互相 平行 。 

现在 绝 大 多 数 的 工业 机 器 人 都 满足 Pieper 准则 的 第 一 个 充分 条 件 : 机 器 人 末端 的 三 个 
关节 的 轴线 相交 于 一 点 ,该 点 通常 被 称 为 腕 点 ,如 图 4. 27 所 示 。 所 以 这 种 构 型 的 工业 机 器 
人 是 有 解析 解 的 。 


图 4.27 某 工业 机 器 人 呈 及 其 机 构 简 图 


目前 ,满足 Pieper 准则 的 第 二 个 充分 条 件 的 机 器 人 较 少 ,最 典型 的 例子 是 SCARA 机 
器 人 ,如 图 4. 28 所 示 , 它 的 前 三 个 旋转 关节 的 轴线 互相 平行 。 从 此 例 可 以 看 出 ,SCARA 机 
器 人 三 关节 平行 的 设计 不 仅 能 满足 机 器 人 在 平面 内 的 快速 移动 ,而 且 可 以 让 机 器 人 有 解 
析 解 。 


图 4.28 某 SCARA 机 器 人 器 及 其 机 构 简 图 


经 研究 发 现 ,Pieper 准则 也 适用 于 带 有 移动 关节 的 机 器 人 。 
目前 ,机 器 人 逆 运 动 学 的 解析 解 方法 主要 有 几何 法 和 代数 法 。 下 面 分 别 对 其 进行 介绍 。 
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1. 几何 法 
下 面 举例 说 明 。 
例 6: 图 4.29(a) 是 一 个 3 自由 度 的 平面 3R 机 器 人 简 图 ,其 末端 位 姿 为 (zy,y,g), 用 几 
何 法 求 该 机 器 人 的 运动 学 逆 解 。 


图 4.29 3 自由 度 的 平面 3R 机 器 人 简 图 


解 : 
定义 各 旋转 关节 角 顺 时 针 转 动 时 为 负 , 逆 时 针 转 动 时 为 正 。 
(1) 设 A 点 的 坐标 为 (zi ,yi), 如 图 4.29(a) 所 示 , 则 可 得 : 
Xi = z— l;cos$ 
yı = y— lsing 
线段 OA 长 度 的 平方 为 : 
10A|*=zi+yi 
Æ hsl ,OA 组 成 的 三 角形 内 ,应 用 余弦 定理 可 解 出 线段 OA 长度 ,因此 可 建立 等 式 : 
| OA |? = zt + yt = ñ + Ë — 2lilzcos(180 —| 0, |) 


W: 
2 2 2 _ 2 
cos0, = rx =. Ë 
162 
2 : s. #29: — Ë 
0, = acos( H2 A 2) 
261 


Tı Yı 需要 满足 三 角形 的 边 长 条 件 : Vz +y Sh as ¿ 
0, 可 能 存在 两 个 解 ,第 二 个 解 如 图 4. 29(b)rB ERR 0, =— 0. 
(2) 参见 图 4. 29(a) ,4. 29(b), 求 0 。 


因为 : 
a = arctan? (yı ,x1) 
aja 23 y il = 
2l zi + yt 
所 以 : 


a 十 pp， 0, < 0 
a= 


a— B 0, = 0 
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(3) 参见 图 4. 29(b) , 求 9;。 
由 对 顶 角 相等 可 得 : ~ 
$— 0 = 0, + 0, 
所 以 : 
加 一 % 一 0 一 0 

由 此 求 得 了 该 机 器 人 的 运动 学 道 解 。 

采用 几何 法 求 机 器 人 的 道 解 需要 特别 注意 不 同 象 限 下 的 符号 问题 ,其 次 要 注意 存在 的 
多 解 问 题 。 几 何 法 求 逆 解 比较 适合 平面 机 器 人 。 

2. 代数 法 

采用 代数 法 求 机 器 人 运动 学 道 解 的 方法 比较 多 ,比如 Pieper FT Paul 方法 外 等 。 
Paul 方法 也 被 称 为 Paul 反 变换 法 ,是 比较 常用 的 机 器 人 运动 学 逆 解 计算 方法 。 该 方法 要 建 
立 机 器 人 的 运动 学 矩阵 方程 ,如 式 (4-6) ,等 式 右边 矩阵 已 知 , 等 式 左边 矩阵 中 的 关节 变量 未 
H. BEHET 一 左 乘 式 (4-6) 和 矩阵 方程 ,然后 从 等 式 两 边 和 矩阵 元 素 中 寻找 并 建立 含有 
单 关节 变量 的 等 式 , 解 出 该 变量 ,再 寻找 并 建立 其 他 的 单 变量 等 式 , 如 果 没 能 解 出 所 有 的 关 
节 变 量 , 则 再 在 等 式 两 侧 左 乘 矩阵 1T ,然后 再 寻找 并 建立 可 求解 的 单 变 量 等 式 …… 直到 
所 有 的 变量 都 解 出 。 

Te) ATga eA TC) =T (4-6) 
Np S P(q y =T RT 
ERIT: Tg) ATC) =T CT CDT 

例 7: K 4.30 是 一 个 3 自由 度 机 器 人 的 简 图 及 其 D-H 参数 表 , 采 用 Paul 反 变 换 法 求 

该 机 器 人 的 运动 学 逆 解 。 


pem evo] a 


ENA 
EEA 
EA 


图 4.30 3 自由 度 机 器 人 简 图 及 其 D-H 参数 表 


很 显然 ,i 该 机 器 人 在 结构 上 满足 Pieper 准则 (1) ,所 以 它 存在 解析 解 。 在 本 章 例 3 中 ， 
已 建立 了 该 机 器 人 的 运动 学 模型 。 
由 该 机 器 人 的 D-H 参数 可 得 各 连 杆 的 齐 次 位 姿 矩 阵 为 : 


cosh simp 0 0 
sinĝ; cos, A -0 
T= 
0 0 JÌ a) 
L 0 0 Gal 
[ cos, —sin0, 0 0 
0 1 有 
iT 一 F 
— sin cosh: 0 0 
L 0 0 Ü 1 
cos0 — sinĝ; 0 0 
5 0 0 — 1 = y 
2 了 = 
sin0, cosh; 0 0 
0 0 0 i! 
假设 机 器 人 末端 坐 标 系 相对 于 基 坐 标 系 的 位 姿 矩 阵 为 : 
w G ag Fa 
. Ne JQ. Q; a P 
ee 
e Gk az Fh 
0 0 0 1 
则 : 
iTi TT =T 
即 . 


cos, cos0; cos0, 一 sin0,sin0, — cos@0,cos@0,sin0, 一 Sin0 cos; 


sin0, cosb cos0, + cos0,sin0, — sin0,cosó,sin0, + cosO cos0; 


一 Sin0 cosh; sin0, sin0, 
0 0 
Hy Os Ay Pa 
n, Oy By P; 
üe Oz âp P; 
0 O 9 1 
采用 Paul 反 变换 法 , 式 (4-7) 两 侧 左 乘 J)T ,得 : 
173T =;¿T à 
即 : 
cos0,cos@0,  — cosĝ: sinb; sin0, a sinf; 
sin0, cos, 0 0 
— cos0,sin0, sin0,sin0;, —cos0, — a;cos@0, 
0 0 0 1 


n, cos0, + n;sin0, o,cos0, + o,sin0, a ,cos0, + a,sinĝ, 


—n,sin0, 二 nycosO! 一 ozsin0 十 oycosO — aa,sin0, + a;cos0, 
n: 


0 


0: 


0 


az 


0 


cos0, sin0, 


sin0, sin0, 


第 4 章 ” 机 器 人 运动 学 ||jp- 95 


a cos0, sinb: 
a sin0, sinf; 
cosh: a, + a; cosh 


0 I 


(4-7) 


P.,cos0, + P,sinĝ, 
一 了 .sin0 + P,,cos0, 
= = P; 
1 
(4-8) 
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由 式 (4-8) 等 号 两 侧 和 矩阵 (2,3) 元 素 相等 ,得 : 
一 Sin0a + cosh a, = 0 ~ 
(1) Æ a; 二 a, 二 0, 则 由 两 矩阵 (1,3) 和 (3,4) 元 素 相等 ,得 : 
sin0, = a,cos0, 十 aysinb = 0 
P. — àr = Wtos, 


Q; P.=a, taz 
maa= o 


Ts P =a — az 

当 0,=0 时 ,根据 式 (4-7) 中 的 (1,1) 和 (1,2) 元 素 相等 ,可 得 : 

` n, = cos, cos, — sin0, sing; = cos(0 + 0, ) 

o, =— cos, sinb, — sin0, cos0;, =— sin(0, + 6.) 
所 以 得 : 

0, +0,=arctan2( —o,; snz) 
即 有 : 
和 一 CI， 0,=arctan2( —o, ,n,)—O, 

其 中 ,Ci 为 任意 角度 。 
当 0, 二 x 时 ,根据 式 (4-7) 中 的 (1,1) 和 (1,2) 元 素 相等 ,可 得 : 

n, =— cosh cosh, — sinf, sinb =— cos(0; — 0, ) 

o, = COSO, sinb; — sinfı cosh, = sinl; — 0, ) 
所 以 得 : 

0,—Ü0, =arctan2(o, ,—n, ) 
即 有 : 
0 =C,, 0; =arctan? (o; ,—n,) + G, 
其 中 ,C; 为 任意 角度 。 
fEa,=a,=0 这 种 情况 下 ,90, 和 0, 有 无 穷 多 组 解 , 只 需 满足 上 述 条 件 即 可 。 
(2) 着 az 天 0 或 者 a, #0 WA : 
0, = arctan2(a,,a,) 或 者 0, = arctan2(—a,, —a,) 
由 式 (4-8) 中 等 号 两 侧 和 矩阵 (3,3) 元 素 相等 ,得 : 
cosh: = a, 
H 3K(4-7) rh 3832 EEG, DAM, DRE, i: 
cos0,sin0, = a, 
Sinb sin0 = a, 
# 0, =arctan2 (a, sa.) , W] sinb >0, B|] sinb = y1— a} , M): 
0, = arctan? ( /1 — aš ,a.) 
Ë Q =arctan?(—a,s— a), I] sinb <0, sinb; =— y1— a} , 则 : 
0, = arctan? (— /1— a ,a.) 

根据 所 得 0, 及 式 (4-8) 中 等 号 两 侧 矩 阵 (3,1) 和 (3,2) 元 素 相等 ,得 : 
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— cosĝ; sin0, = n, 
sin0,sin0, = o. 
所 以 得 : 
0, = arctan2(o, csc0; + — n,.csc0; ) 
所 以 ,该 机 器 人 的 逆 解 为 : 
3Ma,=a,=0,P.,—=a+a, 时 ,0 三 Ci 0, = arctan? (— o, sn:)— C: 0 =0, HP C, 为 任 
意 角度 。 
`4a,=a,=0,P.=a—as, 时 ,0 三 C ,0, = arctan2(o,, —n,) +C, ,0,=x=,# H C, 为 任 
意 角度 。 
当 a, #0 或 者 ,天 0 时 , 则 有 : 
| = arctan2(—a,, —a,),0, = arctan2( 一 /l—aš ,az) |0s = arctan2(o,csc0; , — m, csé0; ) 


0, = arctan2(a,,a,),0, = arctan2( /1— a va,) ,0 = arctan? (o, csch; — ncesc ) 
为 了 验证 上 述 机 器 人 运动 学 逆 解 的 正确 性 ,笔者 编写 了 MATLAB 程序 , 见 本 章 附 录 ， 
它 包 括 运动 学 正解 函数 . 逆 解 函数 和 验证 程序 三 部 分 。 
下 面 分 三 种 情况 来 验证 逆 解 的 正确 性 。 
(1) 设 三 个 关节 角度 为 : 
0 = 30°, 0, = 0°, 0, =— 20° 


用 正解 函数 求 得 机 器 人 的 位 姿 矩 阵 为 : 
0.9848 一 0.1736 0 0 
0. 1736 0.9848 0 0 
H = (4-9) 
0 0 1.0000 32.0000 
0 0 0 1. 0000 


对 于 上 述 位 姿 矩 阵 , 因 为 a, =a, =0 H P. =a, +a: Eit 0, #l 0, 有 无 穷 多 组 解 。 令 
0 二 45" 得 一 组 道 解 为 (45,0, 一 35), 令 9 = 二 0" 得 第 二 组 道 解 (0,0,10)。 将 它们 代入 正解 函 
数 得 机 器 人 的 两 个 位 姿 矩 阵 为 : 


r0.9848 一 0.1736 0 0 F 
„m _ 0.1736 0.9848 0 0 
a 0 1.0000 32. 0000 
h. 0 0 1. 0000 
0.9848 一 0.1736 0 0 J 
„m _ [0.1736 0.9848 0 0 
n = l 0 1.0000 32. 0000 
Lo 0 0 Wasa 
经 对 比 可 知 , 采 用 逆 解 求 得 的 两 个 位 姿 矩 阵 与 预期 的 位 姿 矩 阵 [ 见 式 (4-9)] 完 全 一 致 ， 
因此 可 证 明 所 求 逆 解 的 正确 性 。 


(2) 设 三 个 关节 和 角 订 为， 
0, = SQ" 0, = 180°, 0; w 20° 
用 正解 函数 求 得 机 器 人 的 位 姿 矩 阵 为 : 
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— 0. 6428 — 0. 7660 0 0 
— 0. 7660 0.6428 0 O 志 
T = (4-10) 
0 0 1.0000 — 8. 0000 
0 0 0 1. 0000 


对 于 上 述 位 姿 矩 阵 , 因 为 a, =a,=0 H P. =a, 一 as, 此 时 0 #l 0, 有 无 穷 多 组 解 。 令 
0, 一 45" 时 ,得 一 组 解 为 (45,180 ,一 5) , 令 0 二 0", 得 第 二 组 逆 解 (0,180, 一 50)。 将 它们 代入 


正解 函数 得 机 器 人 的 两 个 位 姿 矩 阵 为 : 
— 0.6428 一 0.7660 0 0 J 
"Wa 一 0.7660 0.6428 0 0 
0 0 1.0000 一 8.0000 
0 0 0 1. 0000 | 
— 0.6428 —0.7660 0 0 村 
sm _ |—0.7660 0.6428 0 0 
eh. 证 0 1.0000 一 8.0000 
0 0 0 1. 0000 | 
经 对 比 可 知 , 采 用 逆 解 求 得 的 两 个 位 姿 矩 阵 与 预期 的 位 姿 矩 阵 [ 见 式 (4-10)] 完 全 一 
致 ,因此 可 证 明 所 求 道 解 的 正确 性 。 
(3) 设 三 个 关节 角度 为 : 
0, = 30°, 0, =— 45°, 0, =— 20° 
用 正解 函数 求 得 机 器 人 的 位 姿 矩 阵 为 : 
0.7465 一 0.2604 一 0.6124 一 12.2474 
r= 080360 0:9374 一 和 35367 -=T 0711 aii 
0.6645 0.2418 0.7071 26.1421 
0 0 0 1. 0000 


对 上 述 位 姿 矩 阵 , 用 反 解 函数 求 得 两 组 逆 解 (210,45,160) 和 (30, 一 45 ,一 20) ,将 它们 分 
别 代 入 正解 函数 ,得 两 个 机 器 人 的 位 姿 矩 阵 为 : 


0.7465 一 0.2604 一 0.6124 — 12.2474] 
api |0.0360 0.9374 一 0.3536 —7.0711 
0.6645 0.248 0.7071 26. 1421 
0 0 0 1. 0000 | 
0.7465 一 0.2604 一 0.6124 — 12.2474] 
amo |0.0360 0.9374 一 0.3536 —7.0711 
0.6645 0.2418 0.7071 26.1421 
0 0 0 1. 0000 | 
Ze WJ E TT 8 , 3 H 25 fg oR 43 WJ P A 28 kB E: j Bi BJ 09 (9 $ E ELR -11 le 
3 , EI k, n] uE BA jr s| j fg BJ TE Bü FE o 
上 述 采 用 Paul AHR D fg ñ | T R BH SL 28 A BJ ë zJ =e fg Er 2 g E: £ gt: 05 , HOR f 
过 程 是 比较 复杂 的 。 


4.3.3 机 器 人 运动 学 逆 解 的 特性 分 析 
机 器 人 运动 学 逆 解 的 特性 分 析 主 要 是 判断 机 器 人 运动 学 道 解 是 否 存在 ,如 果 存 在 ,如 何 


第 4 章 ”机 器 人 运动 学 |l 萝 99 


确定 逆 解 的 数量 以 及 最 优 逆 解 。 
1. 机 器 人 运动 学 逆 解 的 存在 性 
图 4. 31 是 一 个 具有 两 个 旋转 关节 的 平面 2R 机 器 人 简 图 。 
对 于 该 机 器 人 ,采用 几何 法 可 建立 其 运动 学 模型 如 下 ， | sa 
Ë = l cosh, + ¿;cos(0, + 6; ) 2 > 


y = lsin0, + lssin(0 + 0,) 

若 已 知 机 器 人 末端 的 坐标 (zx,y) ,根据 (z,y) 求 解 的 
0, 与 0, 为 机 器 人 的 反 解 。 下 面 先 看 三 个 机 器 人 工作 空 o 
间 的 定义 。 

机 器 人 工作 空间 是 指 机 器 人 末端 抓 手 能 够 到 达 的 图 4.31 平面 2R 机 器 人 简 图 
空间 点 的 集合 ,是 在 笛 卡 儿 坐 标 系 下 描述 的 。 机 器 人 的 
工作 空间 包括 两 类 : 可 达 工 作 空间 和 灵活 工作 空间 。 可 达 工 作 空间 是 指 机 器 人 末端 抓 手 至 
少 能 以 一 种 姿态 达到 的 点 的 集合 。 灵 活 工 作 空间 是 指 机 器 人 末端 抓 手 能 够 以 任意 位 姿 到 达 
的 点 的 集合 。 很 显然 ,灵活 工作 空间 是 可 达 工 作 空间 的 子 集 。 

假设 图 4. 31 所 示 2R 机 器 人 每 个 关节 的 运动 范围 都 是 360 ,机 器 人 的 工作 空间 如 
图 4. 32 所 示 。 如 果 h=, WELA KT A TES EN hth 的 圆 ,灵活 工作 空 
间 是 圆心 点 ; 如 果 4 关 4s, 则 可 达 工 作 空间 是 内 外 半 符 
分 别 是 |4 一 4 | # 1 + 1, 的 圆 环 ,灵活 工作 空间 是 空 
集 。2R 机 器 人 在 可 达 空 间 边界 上 的 任意 点 只 有 一 个 
对 应 位 姿 ,而 在 可 达 空 间 内 部 任意 点 则 有 两 个 对 应 
位 姿 。 

机 器 人 的 工作 空间 与 运动 学 逆 解 的 关系 是 : 如 
果 机 器 人 未 端 位 姿 在 其 可 达 工 作 空间 中 ,其 运动 学 逆 
解 是 存在 的 ,如 果 不 在 其 可 达 空 间 中 , 则 其 逆 解 不 存 
在 。 因 此 ,对 于 图 4. 31 所 示 的 2R 机 器 人 ,如 果 它 的 
. < 末端 位 姿 在 其 可 达 工 作 空间 内 , 则 它 至 少 有 一 个 逆 解 

图 4.32 2R 机 器 人 工作 空间 存在 。 

ER 2R 机 器 人 的 灵活 工作 空间 或 者 为 空 集 或 者 为 
一 个 点 ,很 显然 它 在 平面 内 的 运动 灵活 性 不 是 很 强 。 如 果 想 要 该 平面 机 器 人 具有 更 大 的 灵 
活 工作 空间 , 则 可 增加 1 个 自由 度 变 为 平面 3R 机 器 人 ,该 机 器 人 的 可 达 工 作 空间 等 于 灵活 
工作 空间 。 

2. 机 器 人 运动 学 逆 解 的 数量 与 最 优 解 

如 上 面 的 介绍 ,2R 机 器 人 在 工作 空间 中 (边界 除外 ) 的 任意 位 姿 都 有 两 组 解 。 那 么 机 器 
人 运动 学 逆 解 的 数量 与 什么 因素 相关 呢 ? 

Roth”! $l Tsai 中 等 人 经 研究 发 现 , 机 器 人 运动 学 逆 解 的 数目 与 其 关节 数目 、 连 杆 参 数 、 
关节 变量 范围 三 个 因素 相关 。 一 般 来 讲 , 非 零 的 连 杆 参数 越 多 则 到 达 某 一 位 姿 的 方式 也 越 
多 , 即 运动 学 反 解 数量 也 越 多 。 如 表 4.7 所 示 是 某 6 自由 度 机 器 人 (全 旋转 关节 ) 的 运动 学 
逆 解 数目 与 非 零 连 杆 长 度 的 关系 。 从 表 中 可 看 出 , 当 有 三 个 连 杆 长 度 为 零 时 , 逆 解 数目 最 多 
为 4, 随 着 非 零 连 杆 长 度数 目的 增多 , 逆 解 数目 逐渐 增多 , 当 连 杆 长 度 都 不 为 零 时 , 逆 解 的 数 


100 || 机 器 人 学 一 一 机 构 、 运 动 学 、 动 力学 及 运动 规划 


目 竟 可 达到 16 个 。 ç 
R47 某 6 自 由 度 机 器 人 逆 解 数目 与 非 零 连 杆 长 度 的 关系 
逆 解 数目 
n<4 
n<8 
n<16 


n< 16 


对 应 的 问题 来 了 : 逆 解 的 数目 越 多 越 好 吗 ? 

从 运动 学 逆 解 的 复杂 度 和 计算 的 实时 性 来 讲 , 逆 解 的 数目 并 不 是 越 多 越 好 。 因 为 ,这 些 
解 当中 有 伪 解 。 所 谓 的 伪 解 只 是 在 数学 意义 上 成 立 的 ,但 受 机 器 人 关节 转动 范围 等 因素 的 
限制 ,机 器 人 无 法 实现 相应 的 运动 。 伪 解 的 判定 很 麻烦 。 即 使 所 有 的 解 都 是 可 行 解 ,也 还 需 
要 从 中 选择 一 个 最 优 解 。 例 如 ,9 A n X360" 十 9 都 是 某 个 关节 的 解 ,肯定 需要 的 逆 解 是 0, 这 
样 才能 保证 机 器 人 的 运动 效率 ,而 且 可 以 避免 机 器 人 在 运动 过 程 中 发 生 不 必要 的 碰撞 。 

当然 ,如 果 机 器 人 工作 在 多 障碍 的 复杂 环境 中 , 则 需要 机 器 人 具有 的 逆 解 越 多 越 好 ,这 
样 能 方便 地 找到 满足 机 器 人 无 碰撞 的 逆 解 。 

从 上 面 的 介绍 可 以 看 出 ,在 求 机 器 人 运动 学 道 解 的 过 程 中 , 求 得 逆 解 只 是 第 一 步 ,还 有 
更 重要 的 工作 是 找到 最 优 解 。 

一 般 来 讲 , 确 定 机 器 人 的 最 优 闭 解 需要 采用 选 优 准 则 ,如 距离 最 短 ,力矩 最 小 等 ,可 结合 
具体 的 应 用 情况 选择 和 确定 。 确 定 机 器 人 最 优 逆 解 的 通用 准则 是 “最 短 行程 ?原则 ,即使 每 
个 关节 的 移动 量 都 为 最 小 的 解 ,这 样 可 以 保证 机 器 人 的 快速 响应 。 由 于 典型 工业 机 器 人 的 
前 三 个 关节 大 而 后 三 个 关节 小 ,因此 在 采用 最 短 行程 原则 时 又 可 根据 “多 移动 小 关节 , 少 移 
动 大 关节 ?的 原则 设 定 每 个 关节 的 “加 权 系数 ” ,在 寻 优 时 找到 最 合适 的 逆 解 。 


4.3.4 逆 运 动 学 的 雅 可 比方 法 


几何 法 与 代数 法 都 属于 位 置 级 的 逆 运 动 学 解法 , 即 最 终 所 求 得 的 是 机 器 人 关节 位 置 变 
量 的 解析 表达 式 ,这些 方法 针对 不 同 机 器 人 的 具体 解 算 过 程 是 不 一 样 的 ,而 且 机 器 人 必须 满 
足 一 个 必要 条 件 , 即 机 器 人 的 逆 解 存在 解析 解 。 前 面 已 经 介绍 过 ,并 不 是 所 有 机 器 人 的 逆 解 
都 存在 解析 解 ,因此 ,需要 其 他 的 方法 求解 该 类 机 器 人 的 逆 解 。 下 面 介绍 另外 一 种 机 器 人 的 
逆 运 动 学 解法 : 速度 级 的 雅 可 比方 法 (Jacobian 方法 ) 。 

1. 雅 可 比 和 矩阵 的 定义 及 特点 

速度 级 的 雅 可 比方 法 对 于 不 同 机 器 人 的 闭 解 解 算 过 程 是 一 样 的 ,而 且 不 需要 机 器 人 的 
道 解 存在 解析 解 , 是 一 种 通用 的 逆 运 动 学 求解 方法 。 与 位 置 级 的 
Cy) 逆 运 动 学 求解 方法 相 比 ,该 方法 的 缺点 是 计算 量 大 ( 求 雅 可 比 矩 
阵 )、 速 度 慢 ,优点 是 通用 性 强 。 

图 4. 33 所 示 是 一 个 两 自由 度 的 RP 机 器 人 的 简 图 ， 

对 于 该 RP 机 器 人 ,可 以 采用 几何 法 建立 其 运动 学 模型 , 即 : 

| = rcos0 


K 4.33 RP 机 器 人 简 图 r ,0 C 


y = rsin0 
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对 于 上 列 方程 两 边 分 别 相 对 于 时 间 上 求 导 ,可 得 ， 
Ñ = reos0 一 rbsinb 
Pi C 
$ = rsin0 + rĝcosð 
将 上 式 整 理 成 矩阵 形式 为 : 
N u T aa B 
y| iL rcosð _ sin0j:|; 

上 式 中 ,等 号 左边 是 机 器 人 的 末端 运动 速度 ,等 号 右边 是 机 器 人 的 关节 速度 与 一 个 矩阵 

的 乘积 ,该 矩阵 就 是 机 器 人 的 雅 可 比 矩 阵 ( 虚 线 框 中 ) 。 上 式 可 简写 为 ; 
b = Jd 

雅 可 比 和 矩阵 的 数学 意义 是 ,表示 从 机 器 人 关节 速度 到 机 器 人 操作 速度 的 广义 传动 比 或 
映射 关系 。 需 要 注意 的 是 ,该 广义 传动 比 非 定 值 。 

对 于 任意 机 器 人 的 雅 可 比 矩 阵 可 写成 如 下 通 式 : 

J K£ F. Jam. “= a 
i Ja Ja pe JAn 

这 里 ,m 等 于 机 器 人 工作 空间 的 维 数 , 等 于 机 器 人 关节 空间 的 维 数 ,J 表示 线 速度 的 
传动 比 ,J 表示 角速度 的 传动 比 。 

根据 m 和 nn 的 数值 关系 ,可 将 机 器 人 分 成 以 下 三 种 类 型 。 

D n>m: 元 余 度 机 器 人 ,如 7 自由 度 机 器 人 ， 

© n=m: 常规 机 器 人 ,如 6 自由 度 机 器 人 ; 

© n<m: 欠 驱 动机 器 人 ,如 特殊 结构 的 空间 5 自由 度 机 器 人 。 

对 于 常规 机 器 人 , 雅 可 比 矩 阵 是 方 阵 ,如 果 J 满 秩 ,可 以 直接 利用 公式 9 = J ! p 47 
道 解 计算 。 但 不 是 对 于 所 有 的 关节 角 值 ,J 的 逆 都 存在 ,在 某 些 位 形 时 ,如 果 |J|==0, 则 机 器 
人 处 于 奇异 位 形 或 奇异 状态 ,J 的 逆 不 存在 ,不 能 直接 进行 逆 解 计算 ,需要 采用 特殊 的 解法 。 

对 于 元 余 度 机 器 人 和 欠 驱 动机 器 人 ,由 于 雅 可 比 矩 阵 非 方 阵 ,因此 需要 采用 特殊 的 矩阵 
求 道 方法 (如 广义 逆 法 ) 求 得 雅 可 比 矩阵 的 逆 ,由 公式 g =J p 求 得 机 器 人 道 解 的 特 解 。 

机 器 人 的 雅 可 比 矩 阵 有 下 列 特点 ; 

D 平面 操作 臂 的 雅 可 比 矩 阵 最 多 有 3 行 ; 

D 空间 操作 臂 的 雅 可 比 矩 阵 最 多 有 6 行 ; 

O 具有 nn 个 关节 的 空间 机 器 人 的 雅 可 比 和 矩阵 是 6Xn 阶 ; 

@ 雅 可 比 和 矩阵 的 前 3 行 代表 线 速度 v 的 传递 ,后 3 行 代表 角速度 o 的 传递 ; 

© 雅 可 比 矩 阵 的 每 一 列 代 表 对 应 的 关节 速度 对 机 器 人 末端 线 速 度 和 角速度 的 影响 。 

将 雅 可 比 矩 阵 写成 分 块 的 形式 , 则 机 器 人 末端 的 线 速 度 和 角速度 可 以 表示 成 各 个 关节 
速度 的 线性 函数 。 
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o= Ja à + Ju ge Hst Jr, q, 
a= Ja Q T J an qa tat J an Qh, 
从 上 式 可 以 看 出 ,机 器 人 末端 的 线 速度 和 角速度 分 别 是 各 关节 速度 的 加 权 求 和 , 即 
机 器 人 末端 的 运动 速度 是 各 关节 运动 速度 加 权 后 的 组 合 , 而 且 这 种 组 合 是 非 线 性 的 、 时 
变 的 。 
机 器 人 末端 的 线 速度 和 角速度 也 可 以 写成 微分 移动 和 微分 转动 的 形式 , 即 : 
d = Jan @ + Jez Gt 二 Ju, q, 
£= Ja Qh T Taanga t F Jaa Qa 
机 器 人 的 雅 可 比 矩 阵 有 不 同 的 计算 方法 ,如 矢量 积 方法 、 微 分 变换 方法 等 ,下 面 对 这 两 
种 方法 分 别 做 介绍 。 
2. 雅 可 比 矩 阵 的 矢量 积 求法 
1972 年 Whitney5 基于 和 运动 坐标 系 的 概念 提出 了 雅 可 比 矩 阵 的 矢量 积 求法 。 下 面 介 
绍 这 种 方法 。 
假设 机 器 人 末端 的 线 速度 和 角速度 分 别 用 wv 和 w 表示 。 按 关节 是 移动 关节 和 转动 关节 
两 种 情况 来 介绍 雅 可 比 矩 阵 对 应 列 的 求法 。 
如 图 4. 34 所 示 ,如 果 关 节 i 是 移动 关节 , 则 它 只 使 机 器 人 末端 产生 与 坐标 系 { 引 的 Z 轴 
相同 方向 的 移动 ,产生 的 机 器 人 末端 的 线 速度 为 : 


这 里 ,Z; 是 机 器 人 基 上 坐标 系 下 描述 的 方向 向 量 (单位 向 量 ),Z; 可 由 " 工 得 到 。 
则 雅 可 比 和 矩阵 的 第 i 列 为 : 
Zi 
wS | ] (4-12) 


0 
如 图 4.35 IR IRRI i 是 转动 关节 , 则 该 关节 转动 时 ,使 机 器 人 末端 既 产 生 移动 也 
产生 转动 ,使 机 器 人 末端 产生 的 角速度 为 : 
w= Z, q; 


iz fO 
r Z "Sç. > 
t. ` 
` ` 
`. 


图 4. 34 移动 关节 与 机 器 人 末端 运动 关系 图 4.35 转动 关节 与 机 器 人 末端 运动 关系 
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使 机 器 人 末端 产生 的 线 速度 为 : 
v = (Z, X!P,) d, 
P, RA AR £ (i) Jš 8 #J BL 28 À 3 90 pR £ (n) JEA fJ R t fE 38 AB PRR (0) 中 的 
表达 : 


所 以 s FERT HEE RER SS i 列 为 : 
Z, XP TZ; x GRP) 
£ | Z, |- | Z, l 
mR ERWE , X Br RH Jr ASR 45 BJ ë n] Ek B PE E TEULA A MIIE IRR FIRI OLAFA 
A i H ZK EE JE FU fA gE E E ARETE EA ba A F R o 
3. 雅 可 比 和 矩阵 的 微分 变换 求法 
为 应 用 微分 变换 法 求 雅克 比 和 矩阵 ,应 首先 学 习 微 分 运动 的 等 价 坐 标 变换 ,然后 再 以 此 为 
基础 ,推导 雅 可 比 矩 阵 的 微分 变换 求解 方法 。 
1) 微分 运动 的 等 价 坐标 变换 
(1) 微 移动 和 微 转动 的 变换 矩阵 
假设 机 器 人 的 微 运 动 为 了 D=[Ld 6] ,d= 二 [4d;,d,,d:.]" 是 机 器 人 沿 着 X.Y.Z 三 个 坐 
标 轴 的 微 移动 ,6 二 [6, ,6, ,6.」" 是 机 器 人 绕 X.Y .Z 三 个 坐标 轴 的 微 转动 。 
由 第 3 章 学 过 的 纯 平移 的 齐 次 变换 矩阵 通 式 ,可 得 机 器 人 微 移 动 的 齐 次 变换 通 式 为 ， 


(4-13) 


1 Ü 0 Z. 
o + Fd 
Trans(d,,d,,d.) = I (4=14) 
I o @ 1 Q. 
0 — Q 1 
当 角 度 8 无限 趋 近 于 零 时 , 正 余弦 函数 具有 下 列 特点 : 

sinó ~ ô 
cosô ~ 1 


由 第 3 章 介 绍 过 的 绕 X 轴 旋 转 9 角 的 变换 矩阵 通 式 , 可 得 绕 X 轴 旋 转 ó, 角 的 变换 矩 
阵 为 : 
1 0 0 
o J =Ñ; 
O Q, il 


R(X,6,) ~ 


称 之 为 绕 X 轴 旋 转 的 微分 转动 矩阵 。 
同 理 , 绕 Y 轴 旋 转 6, 角 和 绕 2 轴 旋 转 6. 角 的 微分 转动 矩阵 分 别 为 : 


R(Y,ó)= | 0 1 


0 

1 

~ 二 L: 
R(Z,6:)= |ó, 1 | 

1 
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(2) 绕 过 原点 轴线 的 复合 微分 转动 与 绕 过 原点 矢量 微 转动 的 等 效 变换 
在 第 3 章 介绍 过 一 个 定理 : 任何 一 组 绕 过 原点 轴线 的 复合 转动 总 等 效 于 绕 过 原点 的 某 
一 矢量 的 转动 ,如 图 4.36 所 示 。 对 于 微分 运动 ,该 定理 应 该 依然 成 立 , 即 : 
R(K,60) = R(X,6,)R(Y,6,)R(Z,6.) 
由 绕 单 坐标 轴 的 微分 转动 矩阵 可 得 : 
R(X,6.)R(Y ,6,)R(Z,6.) 


Io O 1 òl = 0 
=o 1 —&| o 1 oll 1 ! 
aa n | aa e À 
1 —6. Ò, 
= ð 1 —ë, (4-15) 
—6, ò 1 图 4.36 坐标 系 绕 过 原点 矢量 转动 


因为 3,,6,,6: 无 限 趋 近 于 零 , 因 此 上 述 计算 结果 示意 图 
省 略 掉 了 它们 的 二 、 三 阶 项 。 从 上 式 可 以 看 出 改变 各 
个 微分 转动 矩阵 的 相 乘 顺序 并 不 会 影响 最 终 的 计算 结果 ,因此 微分 转动 矩阵 相 乘 满 足 乘法 
交换 律 , 即 RCX,6.)RCY,S,) 一 RCIY,0,)RCX,0，) 。 

由 式 (4-15) 可 得 , 绕 坐标 系 三 个 坐标 轴 的 微分 转动 矩阵 的 计算 通 式 为 : 


1 
ë, i =—=& 0 
Rot(5, ,8,,6.) = (4-16) 
l 一 小， ë, Jý 
0 0 0 1 


绕 过 原点 的 矢量 K 二 ki 十 k,j 十 k.k 旋转 0 f 005610 ka EF 308 NU: 
k.k,uersQ +c0 k ,k.uersQ — k.s0 k.k;oers0 + k;,s0 
R(K 0) = n „uers + Rs0 kykyversO+c0 k.kyoers0 — k,s0 
kk vers — ks0 kkversG+ k,s0 E.k.oers0 + c0 
当 80 二 0 Hf ,sinó60<=à0 .cosó0==1 , HARRE K HEFE ó0 角 的 转换 矩阵 通 式 为 : 


1 一 &80 Rb 
R(K,àƏ0) = | k.60 1 —E#Eë0 
— k, ôO k0 1 
如 果 绕 X .Y .Z 轴 的 组 合 转动 等 价 于 绕 过 原点 的 矢量 开 旋转 60 角 , 则 下 列 等 式 成 立 : 
1 一 &60 Ab 1 一 9 Ò, 
R(K,60)= | k80 1 “一 &60|= è 1 =ë 
—k60 k,ô0 1 — ð, ò 1 


= R(X,6,)R(Y ,6,)R(Z,8,) 
由 上 式 可 求 得 等 价 转 轴 K 等 价 微 分 转角 60 与 6,、6,、6: 的 关系 为 : 
ô =k60, 6,=k60, ó. =k.60, 60= /6 TO Të 
由 上 述 公 式 即 可 实现 绕 坐 标 轴 的 复合 微分 转动 与 绕 过 原点 矢量 的 微 转动 之 间 的 等 效 
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(3) 齐 次 矩阵 工 的 微分 和 导数 

齐 次 矩阵 工 可 以 表示 与 之 固 连 的 刚体 的 位 姿 ,那么 工 的 微分 和 导数 就 可 以 表示 与 之 固 
连 的 刚体 的 广义 速度 或 位 姿 的 微分 变化 ,可 表示 为 : 
Te FAD — TI pa CO 


T= lim im 
Ar=0 At Ar-=0 


假设 Ta AE TORY Az 时 间 微 运动 卫 =[d。 d, d. 6; 65，6:]' 后 的 结果 ， 
微 运动 包括 沿 着 三 个 坐标 轴 的 微 移 动 4 二 Ld,,d,,d:] 和 绕 三 个 坐标 轴 的 微 转动 6 = 
[6; ,6, ,6 ,假设 上 述 微 运动 发 生 在 基 坐 标 系 1{10} 中 , 则 
T(t + At) = Trans(d ,d,,d.)Rot(8, 6, 6)。T(Ci) 
dT) = Tt + At) — T) 一 [Trans(d ,d, ,d.)Rot(8, ,9 ,0.) — I] * TO) (4-17) 
E X HX T EAR IE BJ OY PE A : 
A = Trans(d,,d,»,d.)Rot(6, ,6,,6.)—I (4-18) 
则 式 (4-17) 可 简化 为 : 
drt) = A = TD (4-19) 
将 微分 移动 的 变换 通 式 (4-14) 和 微分 转动 的 变换 通 式 (4-16) 代 入 式 (4-18), 得 微分 
算 子 


LL 本 
0 L 0rd Ô- 0 s. “Q Ü Jj © ð Ó: 0 =A d 
=i ear alla a o GL j W a al de a ao 
O @ 0 + 0 0 0 j O 0 @ 1 0 0 0 0 
(4-20) 


利用 式 (4-19) 和 式 (4-20) ,可 以 在 已 知 基 坐 标 系 下 机 器 人 的 位 姿 矩 阵 T ALAE A BJ 6 
运动 D 的 情况 下 , 求 出 机 器 人 位 姿 的 微分 变化 dT) 。 


oio 5 
0 1 10 

例 8: 假设 机 器 人 末端 的 位 姿 算 阵 为 T — A , 基 上 坐标 系 下 机 器 人 末端 的 微 
ooy l 


运动 为 D=[d，d，d。 ô, ô, 80.])'=[0.01 0 0.02 0 0.01 0]r, 求 机 器 人 未 端 
位 姿 的 微分 变化 。 


解 : 
由 微 运动 D 和 微分 算 子 A 通 式 (4-20) ,可 得 微分 算 子 : 
0 D OOL- O 01 
0 0 0 0 
& = 
á =0 01 Ó wT 0.02 
0 0. 0 0 


利用 微分 算 子 A MZEE 工 和 式 (4-19) ,可 求 得 机 器 人 末端 位 姿 的 微分 变化 为 : 
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` 


0 ös MaL 0.01 j W, 5 0.01 O 0 0.01 
0 ó 0 0 E s O Doe Ó 0 O 
at= A T = - 
=001 0 Ó 9.82) W 6 0 0 0 =ġði 0 =. 0 
0 Ó Ó Ba 0 g ¿1 0 0 0 O 


(4) 微分 运动 的 坐标 变换 

假设 D"=[d dd © 6 6] 是 动 坐标 系 {n} 中 的 微 运动 , 则 TCG 十 At) 也 可 以 
表示 成 该 动 坐标 系 中 T(z) 微 运 动 后 的 结果 : 

T(t At) = T(t) * Trans(d’ d'd!) Roto} ,0" ,0’) 
则 : 
dT) = TG + At) — T(t) = TCE) » [Trans(d’,d’,d’)Rot(6’ 62.02) — I] 

定义 相对 于 动 坐标 系 {n} 的 微分 算 子 A” 二 Trans(d’ , d" , di) Rot; ,0 ,67) 一 I, 则 得 : 

dant = TY = A 


1 0 0 Q 外， .二 二 SC Ü ] ò 0-0 Ü =o Os A 
pojezd gle | 
“=l oai all-a ë o o lcooio |- &ë 9 < 

0. 在 通 all Qe —_ 0 20 q). 6 6. @ 1 Ó 7 Q Q 


T(z) 的 微分 变化 dT(2) 可 以 有 两 种 表达 : 
dT(t) = A ° TQ) 
dT) = T(t) ° A” 
一 种 是 基于 基 上 坐标 系 {0) 中 的 微分 运动 描述 的 , 另 一 种 是 基于 动 坐标 系 {n}) 中 的 微分 运 
动 描述 的 。 由 于 两 种 表达 式 描 述 相 同 的 微分 变化 dT(z) , 则 : 
A. TŒ) = T) .A” 
因此 
A"=T() - A- TC) (4-22) 
假设 : 
n. Or dr pz: 
T = n, Oy a, Py 
O © 0 1 
将 微分 算 子 A 通 式 (4-20) 及 T(z) 通 式 代 入 式 (4-22) 中 ,可 得 ， 
ne (Xn) ns Xo) ns Xa) n. ((@XP)+d) 
UX os WWX on Xa) o. CO Py d) 
ae (Xn) a. (Xo) a. (Xa) a» (( X P)+d) 
0 0 0 0 


&" = TH Ae TH = 


(4-23) 
式 中 ,d=[d;,d,,d.j',6 =[6,.,6,,6.]',P=[p: b, p- 
根据 如 下 的 矢量 乘积 性 质 : 
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Q)a ° (bXc)=—b ° (aXc)=b ° (cXa) 
®© a» (aXc)=0 
@ nXo=a,oXa=n,aXn=o 


简化 式 (4-23) ,得 : 


0 一 a 和 wm > CP KN dn 
ó a 0 —ó* n 0G。(PXo) 十 d。o 
Xi (4-24) 
—ó* o 86。7 0 ô. (PXa)+d*»a 
0 0 0 0 


虽然 由 不 同 的 方法 求 得 ,但 式 (4-21) 和 式 (4-24) 都 表示 的 是 A", 所 以 两 矩阵 应 相等 。 
利用 相等 两 矩阵 的 对 应 元 素 相等 ,可 得 动 坐标 系 {n} 和 基 坐 标 系 {10} 中 的 微分 运动 之 间 的 转 
换 关 系 为 : 


[gs Me ny ne PX P Xni, <P%<ynyO rd; 
a ó, by ös CGP23oy, PX, (PX | lZ; 
8: Q. üy 8 XP 01, PX, PX | d, 
= I (4-25) 
人 2 0 @ Q n, m a Ó; 
ó, o Q- g Oz Oy Og Ô, 
|) L. Ó p L a, a, ó: 


ER, (PXn). 二 pn, 一 pyn:，, 据 此 类 推 其 他 类 似 元 素 的 标量 表达 式 。 

如 果 已 知 基 上 坐标 系 {0} 中 的 微分 运动 D=[d, d, d. ð 6，5.]', 则 动 坐标 系 {n} 
中 的 微分 运动 D 二 [d dy dz 6 6 6] 可 用 式 (4-25) 求 出 ,从 而 实现 不 同 坐 标 系 
下 的 微分 运动 的 坐标 变换 。 


n, o, a, P: 
由 通 式 T=|” ””“” | 可 得 R= |n，o， “ee , 则 式 (4-25) 可 简写 为 ; 
Te. Pa Me Ju A Oe Ë p: 
0 @ p 1 
d” R” —R' » S(P)]fd 
[> |- ia R" IM iiz 
0 —p: Ps 
这 里 ,SCP) 为 反对 称 和 矩阵 ,SCP) 一 | b: 0 >p: | ,该 矩阵 具有 下 列 特点 : 
—P, Pa 0 k 


TSCP) =— PX 
S(P) -.ó = P X ë 
如 果 已 知 坐 标 系 {2} 中 的 微分 运动 , 则 基 坐 标 系 {10} 中 的 微分 运动 为 : 


b s 


LE $ 
ñ 0 0 1 10 

例 9: 假设 机 器 人 未 端的 位 姿 矩 阵 为 了 一 | 。 o ' 基 坐标 系 下 的 微分 运动 为 
üü i 
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D=[d; d, d. z 6, 6.]'=[0.01 0 0.02 Ü 0.01 0]', 求 机 器 人 末端 在 坐标 


系 {n} 下 的 等 价 微分 运动 。 $ 
解 : 
Ó j Ó $ 
0 0 10 Ó 1 %@ 
@ @ T w 
H T= ca aA sse- 0 sah 0 1|, 将 S,R,D 代 入 
= § p 1 0“ 0 
0 @ 0 1 


式 (4-26) 可 得 : 


d" RT —R'.S(P)-rd 
| | | = a0 人 


0; R" 
对 上 述 计 算 结 果 做 个 验证 。 由 式 (4-18) 可 得 : 
0 = é ¿ds 0 —0.01 0 —0.03 
r ó: 0 = 122 001 O 0 0.01 
¿ez =0 Q 0 g ~ fo 0 0 0 
0 0 0 0 0 0 0 0 
则 机 器 人 末端 位 姿 的 微分 变化 为 : 
0 1 o 510 —0.01 0 —0.03 
iaki 0 | ' 00 0 0 0.01 
1 @ 10 0|110 0.01 0 0 
O 0 Q 1J LO 0 0 0 
Dare y ó 001 
0 0 $. Q 
~ |o —0.01 0 —0.03 
0 0 0 O 
上 述 计算 结果 与 例 8 的 计算 结果 相同 ,所 以 {?} 坐 标 系 下 的 等 价 征 分 运动 的 计算 结果 是 


正确 的 。 

2) 雅 可 比 和 矩阵 的 微分 变换 求法 

如 果 将 机 器 人 各 个 关节 看 成 参考 系 , 将 机 器 人 末端 看 成 动 坐标 系 , 则 可 根据 式 (4-25) 
或 式 (4-26) 计 算出 雅 可 比 矩 阵 的 各 列 。 下 面 按 关节 是 旋转 关节 或 移动 关节 分 别 介 绍 雅 可 
比 和 矩阵 的 微分 变换 求法 。 

(1) KH i 是 旋转 关节 

如 图 4. 37 RKW i 是 旋转 关节 ,在 坐标 系 {2} PRH i Z 轴 的 微分 转动 为 db,， 
则 坐标 系 {z} 中 关节 的 微分 运动 矢量 为 : 


[| 
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IRP 


六 1 关节 连 杆 天 1 


图 4. 37 旋转 关节 示意 图 


参考 式 (4-25) ,把 坐标 系 (让 看 作 基 坐标 系 , 则 机 器 人 末端 坐标 系 {n} 中 对 应 的 微分 运动 
矢量 为 : 


[di] [n n, n, (PXn), (PXn), (PXn.]|[d4, 
d; 0, Oy O, 《PXO0 (PX; (P Xel |d; 
ga: i üy üe Xar PAO QP GQ). lg 
ô z 0 0 n, M, ft Öz 
ë" 0 0 0, o, 9. Ó, 
DA Lo 0 0 a, g Wy | (6. 
n: 人 
Os Vy Te PAO: PX; PXO 0 
üs <, a, PXW WPX; PXO 0 
5 0 0 0 n, jy M 0 
0 0 Oz Ó, o; 0 
lo 0 Gs Üa ü L d0, 

这 里 了 了 =? 了。 


上 式 中 ,由 于 微分 运动 矢量 D PRA ó, — d0, 不 为 零 , 所 以 机 器 人 末端 坐标 系 {n) 中 的 
微分 运动 矢量 为 : 


d’ | (PX n).] 
C (P X o). 
d! (P Xa). 
ai a (8 
ó, 9; 
a | a 1 
所 以 雅 可 比 和 矩阵 的 第 i 929; 
Pity — mD, 
pioz = ospy 
ka Eh (4-27) 
À A 
0z 
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则 该 列 向 量 中 线 速度 的 传动 比 "J ;和 角速度 的 传动 比 "J 分 别 为 : 
¿sa n,.P y = n, P sz Nz 
，» Ja = . 


一 oz 力 , + o,b. 
—a,b; t asp: 
推论 : 机 器 人 的 旋转 关节 转动 时 , 它 不 仅 会 在 机 器 人 的 末端 产生 移动 分 量 , 同 时 也 会 产 
生 转 动 分 量 。 即 机 器 人 旋转 关节 的 运动 对 于 机 器 人 末端 的 位 置 和 姿态 变化 都 有 作用 。 
(2) 关节 i 是 移动 关节 
如 图 4. 38 所 示 ,关节 i 是 移动 关节 ,在 坐标 系 { 引 中 ,关节 i 沿 2Z; 轴 的 微分 移动 为 dd;， 
则 坐标 系 { 引 中 关节 i 的 微分 运动 矢量 为 : 
0 
0 


` 


i r == 


p- [i]. a- 


7 —— 
T 
Wa, < ij 
° 
II 
——— 


图 4.38 移动 关节 示意 图 


参考 式 (4-25), 把 坐标 系 {z} 看 作 基 坐标 系 , 则 机 器 人 末端 坐标 系 {2} 中 对 应 的 微分 运动 


矢量 为 : 
a. Wa ny Ae (P XR) PX XP 002] dl 
"dó Os Oy Ds PXo)s (PX P Xole 2, 
"da Ws Us Q I (Xa “P xa), “P Xa) | g, 
oe u 0 @% 0 n, H n; DA 
-0 0 0 0 Üz öy O- ó, 
lOa 0 0 O a, a, @s J ló] 
ns ny ne CP X0 (Pn), OP XW] r 0 3 
Ü, Ó, ös WX o, P Xo) QP Ce), 0 
as ü, We PXO: PX; ¿PX a| dd: 
x o 0O 0 n; fy n. 0 
O ğ ğ Oz O 0; 0 
0 0 O a, a, a, | | o] 


这 里 T 二 ?TT。 所 以 机 器 人 末端 坐标 系 {n} 中 的 微分 运动 矢量 为 : 


第 4 章 ”机 器 人 运动 学 | 111 


"d. n: 
"d, o, 
d, a 
= dd; 
"S, 0 
"3, 0 
"ò. 0 
Je u] HE BE 00 96 ; 列 为 : 
n; 
0z 
| (4-28) 
0 
0 
0 


则 该 列 向 量 中 线 速度 的 传动 比 "J 1; 和 角速度 的 传动 比 "J 分 别 为 : 
0 
0 
0 
推论 : 机 器 人 移动 关节 的 运动 ,在 机 器 人 的 末端 只 会 产生 移动 分 量 , 即 机 器 人 移动 关节 
的 运动 只 对 于 机 器 人 未 端的 位 置 改变 有 作用 。 
(3) 微分 变化 法 求 雅 可 比 (Jacobian) 矩 阵 的 基本 步骤 
基本 步骤 如 下 : 
D 计算 各 连 杆 变换 矩阵 ;了 T,1T,… "TT; 
© 计算 末端 连 杆 ”到 各 连 杆 ; 的 变换 矩阵 ?T: 
iT =, T 
iT = TA 


n: 


9 gi gk >= 


a; 


= T =, TT 


oT = TT 
@ 根据 关节 是 转动 关节 或 移动 关节 ,分 别 利用 式 (4-27) 和 式 (4-28) 写 出 雅 可 比 和 矩阵 的 各 列 。 
注意 : 微分 变换 法 计算 的 雅 可 比 矩 阵 是 在 机 器 人 末端 坐标 系 下 的 ,表示 为 "J; 而 矢量 
积 方法 计算 的 雅 可 比 和 矩阵 是 在 参考 系 或 基 坐 标 系 下 的 ,表示 为 J。 它 们 之 间 的 转换 关系 是 : 


F °R l 
== > 
| . | | 一 口 


本 章 主要 介绍 了 机 器 人 的 运动 学 建 模 方 法 及 逆 运 动 学 的 求解 方法 。 和 运动 学 建 模 方法 中 
主要 介绍 了 改进 的 D-H 方法 ,给 出 多 个 例题 。 在 运动 学 求 逆 方 法 中 主要 介绍 了 机 器 人 的 位 


112 | 机 器 人 学 一 -机构 、 运 动 学 、 动力 学 及 运动 规划 


置 级 逆 运 动 学 求解 方法 及 速度 级 的 逆 运 动 学 求解 方法 。 位 置 级 的 逆 运 动 学 方法 主要 介绍 了 
数值 解 方法 和 解析 解 方法 ,重点 介绍 了 Paul 反 变 换 法 ; 速度 级 的 逆 运 动 学 求解 方法 重点 介 
绍 了 雅 可 比 矩 阵 方法 。 
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附录 Pau 反 变 换 法 MATLAB 程序 


Forward PK% ( IE ff ) : 

function T03 = forward(thetal , theta2 , theta3, ai, a2) 

% Forward 函数 根据 theta HiAk 3 % 25 E, 其 中 thetal, theta2 和 theta3 分 别 为 三 个 关节 角 ， 
本 程序 均 以 角度 进行 计算 

T01 = [cosd(thetal), — sind(thetal),0,0; sind(thetal),cosd(thetal),0,0; 0,0,1,al; 0,0,0,1]; 
T12 = [cosd(theta2), — sind(theta2),0,0; 0,0,1,0; — sind(theta2), — cosd(theta2),0,0; 0,0,0,1]; 


T23 = [cosd(theta3), — sind(theta3),0,0; 0,0, — 1, — a2; sind(theta3),cosd(theta3),0,0; 0,0,0,1]; 
T03 = T01 x T12 x T23 ; % 求 末端 姿态 和 矩阵 
Inverse pR 2 (Yf) 


function theta = inverse(T03,al,a2) 

% Inverse 函数 用 于 求解 逆 解 中 各 theta 值 ,参数 T03 为 确定 的 姿态 矩阵 

T = T03 ; 

const = 45; % thetal 的 角度 值 ,用 户 可 赋值 为 任意 值 ,只 有 当 ax = ay = 0 成 立时 使 用 
tol= le- 4; 

if (T(1,3)= =0S&&T(2,3)= =0) % ¥ ax=ay= 0 Wit 
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theta = zeros(1,3); 
if abs(T(3,4) — (al +a2))< tol 
inver_theta2 1 = 0; 
inver thetal 1 = const; 
inver theta3 1 = atan2d( — T(1,2),T(1,1))- inver thetal 1; 
theta(1,:) = [inver thetal 1 , inver theta2 1 , inver_theta3 1] ; 
elseif abs(T(3,4) — (al — a2))< tol 
inver theta2 2 = 180; 
inver_thetal_2 = const; 
inver_theta3 2 = atan2d(T(1,2), - T(1,1)) + inver thetal 2; 
theta(1,:) = [inver thetal 2 ，inver theta2 2 , inver_theta3_2] ; 


end 


else 
82 = H= T3332) (0.5): s 
inver theta2 1 = atan2d(s2,T(3,3)) ; % theta2 的 第 一 组 解 
inver_theta2 2 = atan2d( - s2,T(3,3)) ; % theta2 的 第 二 组 解 
inver thetal 1 = atan2d(T(2,3),T(1,3)) ; %thetal 的 第 一 组 解 
inver thetal 2 = atan2d( - T(2,3), 一 T(1,3)) ; % thetal 的 第 二 组 解 
inver theta3 1 = atan2d(T(3,2) * cscd(inver theta2 1), — T(3,1) *cscd(inver theta2 1) ) ; 
inver theta3 2 = atan2d(T(3,2) x cscd(inver theta2 2), — T(3,1) * cscd(inver theta2 2) ) ; 
theta = zeros(2,3); 
theta(1,:) = [inver thetal 1 , inver_theta2 1 , inver theta3 1] ; 
theta(2,:) = [inver thetal 2 , inver theta2 2 , inver theta3 2] ; 
end 


end 


验证 程序 
clc;clear all;close all; 
al_0 = 12 ;名 取 关 节 长 度 为 12, 可 任意 取 值 
a2_0=20 ;名 取 关 节 长 度 为 20, 可 任意 取 值 
T03 0 = forward(45,180, 一 20,al 0,a2 0); % 调 用 Forward 函数 , 求 末 端 姿态 矩阵 
disp(T03 0); 
theta = inverse(T03 0,al 0,a2 0); % 调 用 Inverse 并 输出 求 得 theta 解 矩 阵 
disp(theta); 
名 名 根据 闭 解 求 得 的 关节 角 求 末端 姿态 矩阵 
[R C] = size(theta) ; 
if R== 1 sg$ 逆 解 为 无 数组 解 情 况 时 ,本 例 中 取 inver thetal 1 = 45 时 的 特 解 
T03_verify = zeros(4, 4); i 
T03_verify = forward( theta(1,1), theta(1,2), theta(1,3),al 0,a2 0 ); 
disp(T03_verify); 
elseif R== 2 $% 道 解 为 两 组 解 情况 
T03_verify = zeros(4,4,2); 
for i=1:2 
TO3_verify(:,:,i) = forward( theta(i,1), theta(i,2), theta(i,3),al 0,a2 0 ); 
disp(T03_verify(:,:,i)); 
end 
end 


机 器 人 和 静 力学 


CHAPTER 5 


机 器 人 项 力 学 主要 是 研究 机 器 人 处 于 静 平 衡 态 时 的 力 系 简化 和 受 力 分 析 问 题 。 所 谓 平 
衡 态 一 般 是 以 地 球 为 参照 系 确定 的 ,是 指 物体 相对 于 惯性 参照 系 处 于 静止 或 匀速 直线 运动 
的 状态 , 即 加 速度 为 零 的 状态 。 本 章 主要 介绍 机 械 臂 在 静止 状态 下 的 受 力 计算 分 析 以 及 广 
义 力 在 不 同 坐 标 系 中 的 转换 问题 。 


5.1 引言 


静 力 学 (Statics) 一 词 是 法 国 数学 家 力学 家 皮 埃 尔 。 伐 里 农 (Pierre Varignon, 1654 — 
1722) 提 出 的 。 静 力学 在 工程 技术 领域 有 着 广泛 的 应 用 ,比如 建筑 物 的 受 力 分 析 、 桥 梁 的 受 
力 分 析 等 。 

按 研究 对 象 的 不 同 , 静 力学 主要 分 为 质点 静 力学 、 刚 体 静 力学 、 流 体 静 力学 三 大 类 。 机 
器 人 静 力 学 属于 刚体 静 力学 的 范畴 。 

按 研究 方法 的 不 同 , 静 力 学 问题 主要 有 图 解法 和 解析 法 。 

图 解法 是 用 几何 作 图 的 方法 来 研究 静 力 学 问题 。 图 解法 获得 的 结果 精确 度 不 高 ,但 计 
算 速 度 快 ,所 以 在 工程 技术 领域 中 应 用 较 多 。 皮 埃 尔 ，。 伐 里 农 在 静 力学 问题 的 图 解法 方面 
做 了 大 量 开创 性 的 工作 。 

解析 法 是 基于 平衡 条 件 式 或 虚 功 原理 用 代数 的 方法 求解 未 知 约束 的 反作用 力 , 计 
算 精 度 高 ,是 一 种 更 具有 通用 性 和 普遍 性 的 方法 ,但 与 图 解法 相 比 ,计算 速度 慢 。 法国 
著名 数学 家 、 力 学 家 拉 格 朗 日 (Joseph-Louis Lagrange,1736 一 1813) 是 静 力 学 解析 法 的 
AEA. 

机 器 人 静 力 学 研究 什么 问题 呢 ? 举例 说 明 。 图 5.1 所 示 的 三 指 灵 巧 手 采用 指 尖 捍 住 鸡 
蛋 静 止 时 ,一般 会 关注 各 关节 的 驱动 力 与 指 尖 作 用 力 之 间 的 关 
系 。 因 为 指 尖 力量 过 大 会 捏 碎 鸡 蛋 , 力 量 过 小 则 捏 不 住 鸡蛋 ， 
鸡蛋 会 脱落 ,因此 这 种 力 的 传递 关系 对 于 灵巧 手 的 设计 和 可 靠 
操作 非常 重要 。 此 外 ,会 关注 各 手指 指 杆 的 受 力 情况 ,用 以 校 
核 各 指 杆 的 刚度 及 计算 各 指 杆 的 变形 情况 。 

在 机 器 人 毅力 学 研究 中 主要 采用 解析 法 ,本 章 也 主要 介绍 a, 
DRADE. ee E 
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5.2 机 械 臂 连 杆 受 力 与 关节 平衡 驱动 力 


机 械 臂 通常 由 连 杆 和 关节 依 序 串联 而 成 ,通过 未 端 与 外 界 发 生 力 的 相互 作用 ,例如 托 举 
重 物 .打磨 工件 等 ,如 图 5. 2 所 示 。 


(a) 托 " 重 物 趾 (b) 打磨 工件 外 
图 5.2 机 械 臂 与 外 界 力作 用 举例 
静 力 学 分 析 所 关注 的 是 机 械 臂 在 静止 状态 时 的 受 力 平衡 问题 ,如 机 械 辟 末端 与 外 界 有 
力 的 作用 时 , 力 是 如 何 从 末端 向 各 连 杆 传递 的 ? 各 关节 需要 施加 多 大 的 驱动 力 才 能 保持 机 
械 臂 的 静 力 平衡 状态 ? 
5.2.1 机 械 臂 连 杆 受 力 计 算 


这 里 将 串联 机 械 臂 的 连 杆 当成 刚体 ,以 其 中 一 个 连 杆 i 为 对 象 对 其 进行 静 力 分 析 , 连 杆 
i 及 其 相 邻 连 杆 之 间 的 作用 力 和 作用 力矩 关系 如 图 5. 3 所 示 。 


图 5.3 连 杆 i 受 力 情 况 


图 5. 3 中 ,在 连 杆 i 上 建立 了 坐标 系 { 让 ,在 连 杆 i 十 1 上 建立 了 坐标 系 {i 十 1} 。 通 常 , 坐 
标 系 {让 和 坐标 系 人 {i 十 1} 分 别 与 连 杆 i 和 连 杆 i 十 1 上 的 D-H 坐标 系 相同 。 

F, 是 连 杆 i 一 1 作用 在 连 杆 i 上 的 力 ,M; 是 连 杆 i 一 1 作用 在 连 杆 i 上 的 力矩 ,作用 点 都 
是 坐标 系 { 让 的 原点 O;。 

Fi,1 是 连 杆 i 作用 在 连 杆 i 十 1 上 的 力 ,M;41 是 连 杆 i 作用 在 连 杆 i 十 1 上 的 力矩 ,作用 
点 都 是 坐标 系 {i 十 1} 的 原点 Oo 

根据 牛顿 第 三 定律 : 两 个 物体 之 间 的 作用 力 和 反作用 力 在 同一 条 直线 上 ,大 小 相等 , 方 
向 相反 , 则 连 杆 i 十 1 作用 在 连 杆 i 上 的 反作用 力 是 一 Fi41, 连 杆 i 十 1 作用 在 连 杆 i 上 的 反 
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力矩 是 一 M+1。 

G, 是 连 杆 i 的 重力 ,作用 在 连 杆 质心 ci 上 ; r 是 连 杆 i 的 质心 在 坐标 系 { 革 中 的 位 置 矢 
量 ; CIP 是 表示 坐标 系 {i 十 1) 的 原点 O;41 在 坐标 系 人 {中 的 位 置 矢量 。 

在 坐标 系 {i} 中 ,以 原点 O; 为 支点 , 连 杆 i 处 于 静 平衡 状态 ,合力 为 零 , 则 可 得 两 个 平衡 
方程 : 

力 平衡 方程 : F: —'Fi +G = 0 

力矩 平衡 方程 : Mi: —'Ma — IP X:F tir XG =0 

ERAD, ERF, F GiM M n WEEER i 表示 变量 在 坐标 系 {i) 中 的 值 。 
由 这 两 个 方程 可 得 连 杆 i 关节 处 受到 的 力 和 力矩 的 递 推 计算 公式 为 : 

ppo 


C5-19 
'M; =M: FP XF in —r XG; 


当 机 械 臂 的 末端 连 杆 与 外 界 有 作用 力 和 力矩 时 ,可 以 采用 式 (5-1) 依 次 递 推 计算 出 从 
末端 连 杆 到 基 座 的 运动 链 中 ,每 个 连 杆 关节 处 受到 的 作用 力 和 力矩 。 
如 果 忽 略 掉 连 杆 本 身 的 重量 , 式 (5-1) 可 以 写成 如 下 形式 : 
'F; ='F in 
ee "Pp KB 
采用 旋转 矩阵 ,将 下;+1 和 "M11 表示 成 从 坐标 系 {i 十 1} 中 的 量 向 坐标 系 { 店 转换 的 形式 ， 
则 上 式 可 改写 为 : 


iF =R oiiF ti 
| (5-2) 
iM; =R +H Mis +P XF; 


式 (5-2) 就 是 在 忽略 连 杆 重力 时 ,机械 臂 各 连 杆 关节 处 受 力 的 递 推 计算 公式 。 


5.2.2 关节 平衡 驱动 力 计 算 


当 机 械 臂 处 于 静 力 平衡 状态 时 ,根据 式 (5-2) 可 求 出 连 杆 站 关 节 处 所 受 的 力 和 力矩 ( 忽 
略 连 杆 重量 ) ,由 此 可 求 出 关节 i 需要 施加 的 平衡 力 或 平衡 力矩 。 下 面 针对 旋转 关节 和 移动 
关节 分 别 做 介绍 。 

1) 旋转 关节 平衡 驱动 力矩 计算 

如 果 不 考 虑 关节 中 的 摩擦 力 , 旋 转 关 节 只 需 提 供 绕 关 节 轴 旋转 的 扭矩 ,其 余 各 个 方 
问 的 力 和 力矩 都 由 关节 的 机 械 结构 承受 了 。 因 此 ,为 保持 连 杆 i 的 静 力 平衡 ,旋转 关节 i 
的 驱动 力矩 为 : 

ë; = Mr +Z, (5-3) 

AP, M; CR” ,是 连 杆 i 关节 处 受到 的 力矩 ,2Z;ER”™' ,是 坐标 系 { 让 的 Z 轴 在 {i} 系 
中 的 矢量 表达 ,M; 的 右上 标 TERE. 

下 面 举例 说 明 如何 计 算 旋 转 关节 的 平衡 驱动 力矩 。 
10 
20 
30 


Mai 
例 1: 假设 连 杆 i 处 于 静 平 衡 状态 ,关节 处 所 受 的 力矩 为 M; 王 N" = , 求 旋 转 关 


zi 
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节 i 需 施加 的 平衡 驱动 力矩 。 


解 : 
依据 式 (5-3) ,旋转 关节 i 需 施 加 的 平衡 驱动 力矩 为 : 
Dome TO 0 
G =M 12; == 1201]. < ai= [C10 2%0 367 =o 
30 i l 


虽然 式 (5-3) 看 着 很 复杂 ,但 由 于 乙 ,=[0 0 1]7, 所 以 只 要 知道 了 2M, Z: 轴 的 分 
量 , 就 很 容易 求 出 旋转 关节 i 的 平衡 驱动 力矩 。 

2) 移动 关节 平衡 驱动 力 计算 

如 果 不 考虑 关节 中 的 摩擦 力 ,移动 关节 只 需要 提供 沿 Z 轴 方 向 的 驱动 力 ,其 余 方向 的 
力 和 力矩 都 由 关节 的 机 械 结构 承受 了 ,所 以 移动 关节 的 平衡 驱动 力 为 : 

A (5-4) 

式 中 , F, € R°! ,是 连 杆 i 在 关节 处 受到 的 力 ,'Z;E R**' ,是 坐标 系 { 让 的 Z 轴 在 {四 系 中 
的 失 量 表达 ,'F; WAEI 工 是 转 置 。 

下 面 举例 说 明 如 何 计算 移动 关节 的 平衡 驱动 力 。 


fa 10 
例 2: 假设 连 杆 i 处 于 静 平 衡 状态 ,关节 处 所 受 的 力 'F; 二 A = x REDIRI ima 
fa 30 
施加 的 平衡 驱动 力 。 
解 : 
依据 式 (5-4) ,移动 关节 i 需 施 加 的 平衡 驱动 力 为 : 
107 ro 0 
g =i g, = RO y = [0 20 30]s j= 
30 1 1 


从 上 述 算 例 可 以 看 出 ,移动 关节 的 平衡 驱动 力 就 是 连 杆 i KEJE, 在 2; 轴 的 分 量 。 

注意 : 在 关节 平衡 驱动 力 /力矩 的 计算 中 ,对 于 旋转 关节 计算 得 到 的 是 力矩 ,单位 是 
N。m( 和 牛顿 米 ) ,对 于 移动 关节 计算 得 到 的 是 力 ,单位 是 N( 和 牛顿)。 

例 3: 如 图 5.4(a) 所 示 , 平 面 2R 机 械 臂 末端 受到 外 界 施 加 的 作用 力 为 F, F 是 在 机 器 
人 未 端 坐标 系 中 描述 的 ,机 械 辟 处 于 静 平 衡 状 态 , 求 各 连 杆 关 节 处 受到 的 力 和 力矩 以 及 各 关 
节 需 施加 的 平衡 驱动 力矩 。 I 


BJ Ë h JE LR AB S JJ F (ft 
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解 : 

如 图 5. 4(b) 所 示 ,首先 在 两 个 关节 上 建立 连 杆 坐标 系 11} 和 人 也) X 和 X, 分 别 在 连 杆 
1 和 连 杆 2 上 ,坐标 系 原点 分 别 位 于 两 个 转轴 中 心 ,Y， M Y 分别 垂直 于 连 杆 1 和 连 杆 2,2 
和 Z, 分 别 过 原点 垂直 于 纸 面 ; 参照 坐标 系 {2) ,在 机 器 人 的 末端 建立 坐标 系 {3) ,坐标 系 {3} 
和 {2} 具 有 相同 的 姿态 ,最 后 在 基 座 上 建立 参考 坐标 系 {0}。 图 中 所 有 坐标 系 的 Z 轴 都 垂直 
于 纸 面 ,为 了 视图 清晰 ,Z 轴 都 未 标注 。 

在 坐标 系 {3} 中 ,FF 可 被 表示 为 FF 二 *f; 二 [ff， OT , 则 利用 式 (5-2) 可 得 连 杆 2 关 


节 处 所 受 的 力 和 力矩 为 : 
i 1 TT 0f Tf- f. 
TEA : i £|- f 
D p 1 0 0 
lz F 0 
0 0 h f; 
再 由 式 (5-2) 递 推出 连 杆 1 关节 处 所 受 的 力 和 力矩 为 : 
GE =y Ü ° Gp fy — Safo 
fi =R = fy, = : €; | + = |s> f, + c; f, 
0 0 14 LO 0 
ce» —s 0 0 Ul p a = 
IM, =}R M, KHP x! fi = |s C2 j x 0 | 十 ° X |S fa ran 
LO 0 11 Ula f; 0 0 
ro 0 0 
=| 0 |+ 0 |- 0 
LL, f, hs, f, + hc; f, hifa "hisa f at aTa 


由 于 机 械 辟 的 两 个 关节 都 是 旋转 关节 ,因此 采用 式 (5-3) 求 得 各 关节 的 平衡 转 矩 为 : 
t = hs, f. + heaf; + Is f; 
9 == L, f, 


Ls Q de ii 


这 样 就 求 得 了 在 机 械 臂 未 端 施加 作用 力 下 时 ,各 连 杆 关节 处 受到 的 力 和 力矩 及 各 关节 
需 提 供 的 平衡 驱动 力矩 。 需 要 注意 的 是 ,本 例 中 ,作用 力 下 是 在 局 部 坐标 系 {3} 中 描述 的 。 

例 4: 对 于 图 5.4(a) 中 所 示 的 平面 2R 机 械 臂 ,如 果 外 界 作用 力 下 是 表示 在 机 械 臂 的 基 
坐标 系 中 的 , 求 机 械 臂 处 于 静 平 衡 态 时 两 个 关节 的 平衡 驱动 力矩 

解 : 

假设 基 坐标 系 中 外 界 作用 力 E= APA ° f, 0]", 该 力 在 机 械 辟 末端 坐标 系 中 
IRS f, 0]", 则 它们 之 间 的 转换 关系 为 : 

fs =IR fs =R”! +° f, 


写成 矩阵 形式 , 即 : 
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坐标 系 10} 与 坐标 系 {3} 之 间 的 姿态 转换 关系 可 描述 为 : 坐标 系 {0} 绕 Z, 轴 旋 转 0, 角 ， 
变 为 坐标 系 {1) ,坐标 系 {1) 再 绕 Z, 轴 旋 转 0, 角 , 则 与 坐标 系 {2) 姿 态 相 同 。 因 为 坐标 系 (3) 
与 坐标 系 {2} 姿 态 相同 ,所 以 坐标 系 143} 到 坐标 系 {0} 的 转换 矩阵 为 : 


n es =$ O Ci =z Ü 
SR =R = R(Z,0.) * R(Z,6,) = |si a Oje |s — G |- se tis Ü 
0 0 l 0 0 1 0 0 1 
式 中 ,三 sin( +02) c12 = cos(0, +60,) 。 
则 : 
Ca S% 0 
全 Sy Ci | 
0 Ó 1 
可 建立 如 下 等 式 : 
[ep Sa. Ü] "is ë 
f= R el f= —sm ë ¿O| * telf 
L. 9 Q J 0 0 
由 上 式 可 求 得 : 
a he 
J 一 sn C12 fs 


将 上 述 求 得 的 f, 和 f. 的 值 代入 式 (5-5), 可 得 : 


Mealy jll 


Flis "DR Ë. y 52 
Lp lz — Si Cig Piy 


may [— hs —lzs2 he + Locu ky 2 
Phr 
由 此 求 得 了 2R 机 械 臂 在 未 端 受 到 作用 力 F( 基 坐标 系 下 描述 ) 时 ,为 保持 静态 平衡 ,各 
关节 需 提供 的 平衡 驱动 力矩 。 


L — Lz si12 LI, C12 


5.3 和 静 力 平衡 方程 与 静 力 映射 分 


静 力 平衡 方程 主要 是 建立 机 器 人 静 平 衡 条 件 下 末端 受 力 与 关节 平衡 驱动 力 之 间 的 映射 
关系 , 静 力 映射 分 析 主 要 是 分 析 关 节 空 间 的 力 与 操作 空间 中 的 力 之 间 的 关联 关系 ,这 两 者 都 
是 机 器 人 更 力学 分 析 中 的 重要 内 容 。 


5.3.1 静 力 平衡 方程 


静 力 平衡 方程 是 采用 烘 功 原理 建立 的 。 
虚 功 原理 是 分 析 静 力学 的 重要 原理 ,也 被 称 为 虚 位 移 原理 ,是 科学 家 J. L. Lagrange 于 
1764 年 提出 的 ,当时 他 才 28 岁 。 该 原理 是 : 一 个 原 为 静止 的 质点 系 , 如 果 约 束 是 理想 双 面 
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定常 约束 , 则 系统 继续 保持 静止 的 条 件 是 所 有 作用 于 该 系统 的 主动 力 对 作用 点 的 虚 位 移 所 
做 功 的 和 为 零 。 Š 

虚 位 移 指 的 是 物体 被 附加 的 满足 约束 条 件 及 连续 条 件 的 无 限 小 可 能 位 移 。 虚 位 移 的 
“ 虚 ” 字 表明 它 可 以 与 真实 的 结构 受 力 而 产生 的 真实 位 移 无 关 , 可 以 是 由 其 他 原因 (如 温度 变 
化 、 外 力 系 作用 或 其 他 干扰 ) 造 成 的 满足 位 移 约束 、 连 续 条 件 的 可 能 几何 位 移 。 由 于 虚 位 移 
是 无 穷 小 位 移 , 所 以 在 产生 虚 位 移 过 程 中 不 会 改变 原 受 力 平衡 体力 的 作用 方向 与 大 小 , 即 受 
力 平衡 体 的 平衡 状态 不 会 因 产生 虚 位 移 而 改变 。 

真实 力 在 虚 位 移 上 做 的 功 称 为 虚 功 。 数 学 表示 上 , 虚 功 是 力 矢量 或 力矩 与 虚 位 移 的 点 
积 。 对 于 力矩 , 虚 功 为 .WW 二 rc”。6g; HFI BD W= F + ód, 

针对 不 同 的 对 象 , 虚 功 原理 演变 出 不 同 的 细 分 原理 ,如 刚体 体系 的 虚 功 原理 、 变 形体 系 
的 虚 功 原理 等 。 机 器 人 静 力 学 分 析 中 采用 的 虚 功 原理 属于 刚体 体系 的 虚 功 原理 。 刚 体 体 系 
的 虚 功 原理 : 假设 在 满足 理想 约束 的 刚体 体系 上 作用 任何 的 平衡 力 系 ,刚体 体系 发 生 满 足 
约束 条 件 的 无 限 小 位 移 , 则 主动 力 在 位 移 上 所 做 的 虚 功 总 和 恒 为 零 。 功 是 表示 能 量 的 物理 
量 , 计 量 单位 是 焦耳 (Joule) , 静 力 平衡 系统 中 各 分 系统 所 做 的 虚 功 必须 单位 一 致 。 

采用 虚 功 原理 可 以 将 机 器 人 操作 空间 中 做 的 虚 功 与 机 器 人 关节 空间 中 做 的 虚 功 建立 等 
价 关系 ,由 此 可 推导 机 器 人 的 静 力 平衡 方程 。 

假设 有 一 个 关节 的 机 械 辟 ,将 机 械 臂 末端 所 受到 的 力 和 力矩 用 一 个 六 维 矢量 表 示 为 : 


m 


这 里 五 被 称 为 广义 力 , 即 不 细 究 它 是 力 、 力 矩 还 是 力 和 力矩 的 组 合 。F R fE ËA TER Z 
下 描述 的 。 

将 各 关节 的 驱动 力 表示 成 n ERE: 

t= [rm n "e n] 

这 里 也 不 关注 它 是 驱动 力 还 是 驱动 力矩 。 

对 于 该 机 械 臂 ,将 关节 驱动 力 撩 量 z 看 成 系统 的 控制 输入 ,未 端 产生 的 广义 力 五 作为 系 
统 的 控制 输出 ,采用 虚 功 原理 推导 它们 之 间 的 关系 。 

在 虚 功 原理 中 , 虚 位 移 被 定义 为 满足 机 械 系 统 的 几何 约束 条 件 的 无 限 小 位 移 。 令 机 械 
臂 各 关节 的 虚 位 移 为 oq [q q. … aT ， 则 各 关节 所 做 的 虚 功 之 和 为 : 

w=Tt'*6g = nq H + r,Óq, 

DPLI K m BJ EAA D—= [d, d, d. ó, Ó, ò] D EERME F uË 

的 , 则 机 械 臂 末端 所 做 的 虚 功 为 : 
w= F < p = fdr +t f gy + fale md ms md 

根据 虚 功 原理 ,在 机 械 臂 静 平衡 情况 下 ,由 任意 虚 位 移 产生 的 虚 功 和 为 零 , 即 关节 空间 

虚 位 移 产生 的 虚 功 等 于 操作 空间 虚 位 移 产 生 的 虚 功 , 由 此 可 得 : . 
tr'*64=F + D 
在 机 器 人 运动 学 中 ,由 于 也 =J. óq ,所 以 上 式 可 表示 为 : 
r'e ôq = F + J ° ôq 
消 掉 òq ,可 得 
#T=F7 + J 
消 掉 转 置 ,得 到 机 械 臂 的 静 力 平衡 方程 为 : 
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r= JF (5-6) 

式 中 ,J" 被 称 为 “ 力 雅 可 比 ”, 它 建立 了 静 力 平衡 条 件 下 机 械 臂 的 末端 受 力 与 各 关节 了 驱 
动力 之 间 的 映射 关系 。 

_ 由 式 (5-6) 可 得 出 两 点 结论 : 

(1) 在 仅 考虑 机 械 辟 关节 驱动 力 和 末端 作用 力 的 情况 下 ,机 械 辟 保持 静 平衡 的 条 件 是 
关节 驱动 力 满足 式 (5-6); 

(2) 在 机 械 臂 静 力 平衡 状态 下 ,机 器 人 的 力 雅 可 比 和 矩阵 是 它 运 动 雅 可 比 矩 阵 的 转 置 。 

需要 注意 的 是 ,上 述 结论 只 有 在 忽略 机 械 辟 重 力 的 条 件 下 才能 成 立 。 

例 5: 如 图 5.5 所 示 的 2R 机 械 臂 ,其 末端 受到 外 界 施加 的 作用 力 为 下 ,下 是 在 基 坐 标 系 
下 描述 的 ,该 机 械 辟 处 于 更 平衡 状态 , 求 各 个 关节 的 平衡 驱动 力矩 。 

解 : 

如 图 5.5 所 示 ,2R 机 械 臂 的 基 坐 标 系 是 XOY, 定 义 两 个 关 
WHA O 和 狐 , 顺 时 针 方 向 为 负 , 逆 时 针 方向 为 正 。 建 立 该 机 顺 
人 的 运动 学 方程 : 

i = hcos0 + L,cos(0, + 0, ) 


ka: l sinô; + ¿,sin(0, - 0, ) 
上 式微 分 后 求 得 机 械 臂 的 雅 可 比 和 矩阵 为 ， 


J | lisi lz $12 a 
E lici + lcs Lazciz 


则 力 雅 可 比 矩 阵 为 ， 
— lisi 一 /zsiz lici F lz 
ha! | 
— lisz lacls 
则 各 关节 的 平衡 驱动 力矩 为 : 
T “s — lisi — ls lici cu 
aak oin | iie LG |: 


从 上 述 计算 可 以 看 出 ,通过 静 力 平衡 方程 可 以 很 方便 地 求 得 静 平衡 状态 下 2R 机 械 臂 
各 关节 的 平衡 驱动 力 , 力 雅 可 比 和 矩阵 J" 可 由 机 器 人 的 运动 学 雅 可 比 矩 阵 J 转 置 求 得 。 

例 4 和 例 5 求 的 都 是 2R 机 械 臂 的 关节 平衡 驱动 力矩 ,所 用 的 方法 不 同 , 但 结果 是 相 
同 的 。 


5.3.2 静 力 映射 分 析 


图 5. 6 表示 的 是 机 器 人 操作 空间 作用 力 下 与 机 器 人 关节 驱动 力 r 之 间 的 静 力 映射 关 
系 。 假 设 机 器 人 操作 空间 是 m 维 的 ,机 器 人 关节 空间 是 维 的 ,由 于 雅 可 比 和 矩阵 J 与 机 器 人 
的 位 形 g 相关 ,JER"",qgER' ,所 以 力 雅 可 比 矩 阵 J! 也 是 与 机 器 人 的 位 形 q 相关 的 ， 
PER, 

静 力 平衡 方程 r = J' EÆA m 维 的 操作 空间 到 维 的 关节 空间 的 线性 映射 , 即 对 于 给 
定 的 机 器 人 末端 作用 力 和 机 器 人 位 形 q ,机 器 人 的 关节 平衡 驱动 力 r 是 唯一 确定 的 ,但 是 
对 于 给 定 的 机 器 人 关节 驱动 力 r 和 机 器 人 位 形 g ,机 器 人 未 端 作 用 力 正 是 不 唯一 的 。 机 器 人 
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也 线性 映射 


5.6 静 力 映射 关系 


末端 作用 力 FE R" 通过 ,三 映射 出 的 区 域 设 为 r,t 是 rt 的 值 域 空间 的 子 集 ,而 zr 的 值 域 空 
间 是 由 tr 和 zt 两 部 分 组 成 ,r 表示 的 是 关节 驱动 力 的 余 量 。F, 是 下 的 值 域 空间 的 子 集 , 代 
表 的 是 一 个 特殊 的 力 空间 , 它 与 机 器 人 的 奇异 位 形 相 对 应 , 当 机 器 人 
处 于 奇异 位 形 时 ,机 器 人 的 末端 作用 力 与 机 器 人 关节 平衡 驱动 力 之 间 
不 能 建立 线性 映射 关系 ,FF 空间 的 所 有 变量 都 映射 为 r 的 零 空间 或 堆 
点 , 即 机 器 人 处 于 奇异 位 形 时 ,机 器 人 末端 作用 力 都 被 机 器 人 机 构 本 
体 承受 了 ,并 不 需要 关节 产生 平衡 驱动 力 。 

5. 7 所 示 的 是 一 个 两 自由 度 的 机 械 臂 处 于 奇异 状态 ,此 时 可 以 
用 很 小 的 关节 力矩 平衡 非常 大 的 末端 作用 力 F. B. F 的 增 大 并 不 会 
导致 关节 驱动 力矩 的 增 大 。 其 原因 就 是 在 奇异 状态 下 ,机械 臂 末端 的 
作用 力 都 映射 到 关节 驱动 力 抢 的 零点 上 了 。 


5.4 静 力 学 的 逆 问 题 


机 械 臂 处 于 静 力 平衡 时 ,关节 驱动 力 r 和 外 界 作 用 力 F 之 间 的 存在 关系 式 : z 二 J"F, 这 
种 从 外 界 作 用 力 FF 到 关节 驱动 力 r 之 间 的 映射 被 称 为 静 力 学 的 原 问 题 。 静 力学 的 逆 问 题 
是 : 如 果 已 知 关节 驱动 力 r ,如 何 求 作 用 力 F? 

很 显然 , 静 力 学 的 首 问题 与 力 雅 可 比 矩 阵 J 直接 相关 。 

(1) 如 果 雅 可 比 矩 阵 J 是 方 阵 且 J' 的 道 存 在 , 则 可 直接 求 出 静 力 学 的 道 解 : F = 
Qy Aw e, 

(2) WRJ PEDE. RJS 的 道 不 存在 , 则 下 的 值 不 确定 , 即 t 与 F 之 间 无 法 建立 直接 
映射 关系 。 可 采用 最 小 二 乘 的 方法 求 得 下 的 一 个 特 解 : F= (JJ!) e 。 

静 力 学 逆 解 的 主要 用 途 是 在 机 械 臂 设计 时 ,由 各 关节 的 驱动 力矩 推导 出 机 械 臂 的 静 负 
荷 。 如 果 对 机 械 臂 的 静 负 荷 进行 遍历 计算 , 则 可 以 确定 机 械 臂 的 静 负 荷 的 范围 ,从 而 指导 机 
械 臂 的 关节 驱动 选 型 。 


5.5 力 与 力矩 的 坐标 变换 
在 机 器 人 中 , 力 与 力矩 也 需要 在 不 同 的 坐标 系 之 间 进 行 转换 ,从 而 满足 测量 、 分 析 等 不 


同 的 需求 。 
假设 六 维 力 和 力矩 矢量 表示 为 广义 力 下。 如 果 已 知 坐 标 系 47) 中 的 广义 力 值 为 卫 ,如何 
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求 它 在 另外 一 个 坐标 系 (四 中 的 值 'F 呢 ? 
下 面 利 用 虚 功 原理 推导 广义 力 从 坐标 系 {j}) 到 坐标 系 { 让 的 变换 。 
假设 坐标 系 {7} 中 的 虚 位 移 和 作用 力 分 别 为 ';D 和 ’'F, 它 们 在 坐标 系 { 沾 中 对 应 的 虚 位 移 
和 作用 力 分 别 为 卫 AEF. 
根据 虚 功 原理 : 外 力 和 等 效力 所 做 的 虚 功 之 和 为 零 , 可 得 : 
FT D 一 FT jD (5-7) 
由 微分 运动 的 坐标 变换 式 (4-26) 可 得 iD 到 'D 的 转换 关系 为 : 
m" —R 。 + 
“Ty = “D 
0 iR 
则 : 
ñ SP) i | 
iD = “D 
0 iR 
将 上 式 代 入 式 (5-7) 中 ,可 得 : 


I PR SCPI R 
iFT D = FT » `D 
0 


IR 
LI 2638083232 D ,得 : 
A -n 
iF! =i FT 
0 İR 
将 上 式 消去 转 置 ,得 广义 力 从 坐标 系 {7} 到 坐标 系 (让 的 转换 方程 为 : 


iR O. 
= |a |F (5-8) 
P iP):R Hi 
式 (5-8) 即 为 两 坐标 系 中 的 广义 力 转换 公式 。 
式 (5-8) 也 可 简写 为 : 
iF =T iF (5-9) 
j 0 
S(®P)iR $R 
例 6: 如 图 5.8 所 示 ,一 个 带 有 腕 部 6 维 力 传感器 的 机 器 人 通过 工具 打磨 工件 , 力 传 感 
器 检测 出 的 6 维 力 为 "下 ,计算 工具 与 工件 之 间 的 作用 力 。 


mur] aar nsen. 


图 5.8 机 器 人 打磨 工件 示意 图 
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解 : 
假设 腕 部 传感器 所 在 的 坐标 系 为 {W} ,工具 顶部 所 在 的 坐标 系 为 {T) , 5 kn Z (W ) #l] Aë 
标 系 {T} 的 齐 次 变换 和 矩阵 为 : 
IR "PP 
a | si J 
由 矩阵 YT 可 得 到 YR 和 ?已 ,由 "7P 可 得 到 SC?P), 再 根据 广义 力 坐 标 变换 式 (5-8) ,可 
得 工具 与 工件 之 间 的 作用 力 为 : 


. wR Üi sP. 
231 asa a i 
这 种 广义 力 的 坐标 变换 方法 可 通过 间接 测量 机 器 人 与 外 界 的 作用 力 , 然 后 通过 坐标 变 
换 的 方式 实现 对 无 法 直接 安装 力 传感器 的 部 位 的 力 测量 ,具有 重要 的 实用 价值 。 


5.6 小结 


本 章 首先 介绍 了 静 力 平衡 条 件 下 串联 机 械 臂 的 连 杆 受 力 及 关节 平衡 驱动 力 的 计算 方 
法 ,在 此 基础 上 讨论 了 静 力 平衡 方程 . 静 力 映射 及 静 力学 逆 问 题 ,最 后 介绍 了 广义 力 在 不 同 
坐标 系 之 间 的 转换 公式 。 
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CHAPTER 6 


动力 学 是 理论 力学 的 一 个 重要 分 支 ,主要 研究 作用 于 物体 的 力 与 物体 运动 的 关系 。 机 
器 人 是 一 个 具有 多 输入 和 多 输出 的 复杂 动力 学 系统 ,存在 严重 的 非 线性 ,需要 非常 系统 的 方 
法 对 机 器 人 进行 动力 学 研究 。 常 用 的 机 器 人 动力 学 建 模 方法 比较 多 ,如 Lagrange (hi É B 
H) 动力 学 方法 .Newton-Euler( 牛 顿 - 欧 拉 ) 动 力学 方法 .Gauss( 高 斯 ) 动 力学 方法 Kane( 凯 
恩 ) 动 力学 方法 .Roberson-Wittenburg ( 罗 伯 逊 - 魏 登 保 ) 动 力学 方法 等 。 本 章 主 要 介绍 
Lagrange 动力 学 方法 、Newton-Euler 动力 学 方法 及 Kane 动力 学 方法 。 


动力 学 研究 的 对 象 是 运动 速度 远 小 于 光速 的 宏观 物体 ,而 原子 、 亚 原子 和 粒子 的 动力 学 
属于 量子 力学 ,可 比拟 光速 物体 的 动力 学 则 属于 相对 论 力学 。 动 力学 主要 包括 质点 动力 学 、 
质点 系 动力 学 、 刚 体 动 力学 、 达 朗 贝 尔 原理 等 内 容 。 

1687 年 著名 科学 家 牛顿 出 版 了 《自然 哲学 的 数学 原理 》(Mathematical Principles of 
Natural Philosophy) 一 书 , 提 出 了 牛顿 第 二 运动 定律 ,指出 了 力 、 加 速度 ,质量 三 者 之 间 的 
关系 。 牛 顿 第 二 运动 定律 是 动力 学 的 基础 和 核心 。18 世纪 ,瑞士 学 者 欧 拉 (Leonhard 
Euler,1707 一 1783 年 ) 引 入 了 刚体 的 概念 并 把 牛顿 第 二 运动 定律 推广 到 刚体 。 牛 顿 第 二 运 
动 定律 提出 100 年 后 ,法 国 数学 家 J. L. Lagrange( 拉 格 朗 日 ) 建 立 了 能 应 用 于 完整 系统 的 
Lagrange 方程 。 该 方程 不 同 于 牛顿 第 二 运动 定律 的 力 和 加 速度 的 形式 ,而 是 用 广义 坐标 为 
自 变量 ,通过 Lagrange 函数 来 表示 。 应 用 Lagrange 方程 研究 刚体 动力 学 问题 比 应 用 牛顿 
第 二 运动 定律 更 方便 。 

机 器 人 动力 学 研究 的 是 机 器 人 的 运动 和 作用 力 之 间 的 关系 。 如 图 6.1 所 示 , 两 个 机 器 
人 分 别 在 做 单 腿 站 立 和 在 布 满 障碍 物 的 不 平地 面 行走 ,为 了 使 机 器 人 保持 身体 平衡 不 摔 倒 ， 
则 机 器 人 的 运动 需 满足 某 种 特定 的 力学 平衡 关系 ,很 显然 这 不 是 通过 运动 学 和 静 力 学 能 够 
实现 的 ,而 是 需要 通过 对 机 器 人 进行 动力 学 控制 来 实现 。 

机 器 人 动力 学 问题 包括 正 问 题 和 逆 问 题 两 大 类 , 

动力 学 的 正 问题 是 对 于 给 定 的 关节 驱动 力 / 力 抢 ,求解 机 器 人 对 应 的 运动 。 需 要 求解 非 
线性 的 微分 方程 组 ,计算 复杂 ,主要 用 于 机 器 人 的 运动 仿真 。 

动力 学 的 遂 问 题 是 已 知 机 器 人 的 运动 ,计算 对 应 的 关节 驱动 力 / 力 矩 , 即 计算 实现 预定 
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(a) 机 器 人 单 腿 站 立 叫 (b) 机 器 人 在 障碍 地 面 行走 四 
图 6.1 机 器 人 动力 学 举例 


运动 所 需 施 加 的 力 / 力 矩 。 不 需要 求解 非 线性 方程 组 ,计算 相对 简单 ,主要 用 于 机 器 人 的 运 
动 控制 。 

机 器 人 动力 学 的 用 途 可 以 概括 为 以 下 三 个 方面 : 

(1) 为 机 器 人 设计 提供 依据 : 能 计算 出 实现 预定 运动 所 需 的 力 /力矩 。 

(2) 机 器 人 的 动力 学 仿真 : 能 根据 连 杆 质量 、 负 载 \ 传 动 结构 进行 动态 性 能 仿真 。 

(3) 实现 机 器 人 的 最 优 控制 : 能 优化 性 能 指标 和 动态 性 能 ,调整 伺服 增益 。 


6.2 Lagrange 动力 学 方法 


Lagrange 动力 学 方法 是 研究 机 器 人 动力 学 问题 的 一 种 重要 方法 ,下 面 介 绍 如 何 构建 
Lagrange 动力 学 模型 以 及 对 Lagrange 动力 学 方程 进行 分 析 。 


6.2.1 Lagrange 动力 学 建 模 


Lagrange 是 分 析 力 学 的 创立 者 。1788 年 ,Lagrange 出 版 了 其 名 著 《 分 析 力 学 》, 在 总 结 
历史 上 各 种 力学 基本 原理 的 基础 上 ,引进 广义 坐标 的 概念 ,建立 了 Lagrange 方程 ,把 力学 体 
系 的 运动 方程 从 以 力 为 基本 概念 的 牛顿 形式 ,改变 为 以 能 量 为 基本 概念 的 分 析 力 学 形式 , 黄 
定 了 分 析 力 学 的 基础 。Lagrange 方程 是 运用 达 朗 贝尔 原理 得 到 的 力学 方程 ,和 牛顿 第 二 运 
动 定律 等 价 , 但 Lagrange 方程 具有 更 普遍 的 意义 ,适用 范围 更 广泛 ,而 且 选 取 恰 当 的 广义 坐 
标 可 以 使 Lagrange 方程 的 求解 大 大 简化 。 

Lagrange 动力 学 有 一 个 基本 假设 : 具有 个 自由 度 的 系统 ,其 运动 状态 完全 由 个 广 
义 坐 标 及 它们 的 广义 速度 决定 。 或 者 说 ,力学 系统 的 运动 状态 可 由 一 个 含有 广义 坐标 和 广 
义 速度 的 函数 描述 。Lagrange 动力 学 方法 就 是 基于 该 假设 ,能 以 最 简单 的 形式 求 得 非常 复 
杂 系 统 的 动力 学 方程 ,而 且 具 有 显 式 结构 ,是 一 种 很 实用 的 动力 学 建 模 方法 。 下面 对 
Lagrange 动力 学 方法 进行 详细 介绍 。 

在 Lagrange 动力 学 方法 中 有 一 个 很 重要 的 变量 : Lagrange 函数 工 , 它 被 定义 为 任何 机 
械 系 统 的 动能 E, MAREE, 之 差 : 

Lesi 
在 Lagrange 动力 学 方法 中 ,动能 和 势能 可 以 用 任意 选取 的 坐标 系 来 表示 ,不 局 限于 笛 


第 6 章 ” 机 器 人 动力 学 |P 127 


卡 儿 坐标 系 。 
假设 自由 度 机 器 人 的 广义 坐标 为 gq;(i 二 1,2,…,n) , 则 该 机 器 人 的 动力 学 方程 为 : 
E AL al 
y =s ET s (6-1) 


式 中 ,f; 是 机 器 人 的 广义 驱动 力 , 若 q. 是 直线 运动 变量 , 则 f; 表示 的 是 力 , 若 q, 是 角度 
变量 , 则 f, 表示 力矩 ; q, 是 机 器 人 关节 i 的 广义 速度 。 

W L=E,—E, 代入 到 式 (6-1) ,得 : 

2i (3 一) 一 (42E 一 2 
dż 9 q, 9g; 

由 于 势能 E, 不 显 含 4 G 二 1,2,…,n) ,因此 ,Lagrange 动力 学 方程 也 可 以 写成 : 

3E, 2E, , aE, 
f. = i ag 9g: 9g; 

从 式 (6-2) 可 以 看 出 ,Lagrange 动力 学 方程 是 基于 能 量 的 ,与 纯 基 于 力 的 牛顿 第 二 运动 
方程 是 完全 不 同 的 形式 。 由 于 系统 的 广义 驱动 力 仅 与 系统 的 动能 .势能 和 广义 位 置 .广义 速 
度 相关 ,因此 该 方法 特别 适合 用 于 多 自由 度 复杂 系统 的 动力 学 建 模 ,而 且 不 受 系统 结构 复杂 
度 的 影响 ,这 也 是 该 方法 被 广泛 应 用 的 原因 。 

例 1: 图 6.2 所 示 是 一 个 RP 机 器 人 的 机 构 简 图 ,每 个 连 杆 的 质心 皆 位 于 连 杆 末端 , 求 
其 Lagrange 动力 学 方程 。 

解 : 

首先 ,分 析 一 下 该 机 器 人 。 它 有 两 个 带 有 质量 的 运动 连 
杆 , 其 自由 度 为 2, 有 两 个 广义 坐标 9 和 x。 为 了 求 得 该 机 器 人 G 
的 Lagrange 动力 学 方程 ,需要 计算 其 动能 和 势能 ,而 动能 和 势 4 
能 是 与 两 个 质心 的 位 置 和 速度 相关 的 。 建 立 如 图 6.2 所 示 的 g 
基 坐 标 系 ,并 标注 各 参量 。 A Y 

(1) 求 连 杆 质心 的 位 置 和 速度 图 6.2 RP 机 器 人 机 构 简 图 

为 了 写 出 连 杆 1 和 连 杆 2( 质 量 m 和 m, ) 的 动能 和 势能 ， 
需要 知道 它们 的 质心 在 基 坐 标 系 XOY( 参 考 系 ) 中 的 位 置 和 速度 。 

连 杆 1 的 质心 mi 的 位 置 是 : 

i = r; cos? 


dag 2q; 


(6-2) 


AEC 


Jr = f] sin0 


对 上 式 相 对 于 时 间 上 求 导 数 ,可 得 质心 m 的 速度 是 : 
| =— rising6 
yı = fy cos00) 
Z; = rcos0 


š | r€ C 


ys = rsin0 


连 杆 2 的 质心 m; 的 位 置 是 : 


质心 ms 的 速度 是 : 
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| = rcosb 一 7sihb6 


Ys = rsing 十 rcosb6 
(2) 求 机 器 人 的 总 动能 


质量 为 m EEX v 的 质点 的 动能 为 E,= m, 分 别 求 出 连 杆 1 和 连 杆 2 的 动能 。 


连 杆 1 的 动能 为 : 

En = mvi == Fm (好 十 党) = > lË 
ÉE FF 2 的 动能 为 : 

Ep = mÚ ul = > mÚ Gi +y) = Tm += 02) 
所 以 ,机 器 人 的 总 动能 为 : 

E, = Brt Eg = ym iË 十 Fm r 十 m ti 


(3) 求 机 器 人 的 总 势能 
质量 为 m、 高 度 为 h 的 质点 的 势能 为 E, 二 mgh。 分 别 求 出 连 杆 1 和 连 杆 2 的 势能 。 


连 杆 1 的 势能 : 

En =m grisin0 
连 杆 2 的 势能 : 

Ep = m; grsin0 
机 器 人 的 总 势能 为 : 


E,= E, + E,; =m g7iSin0 十 72 grsin0 
(4) 求 机 器 人 的 Lagrange 动力 学 方程 
根据 式 (6-2) ,分 别 计算 关节 1 和 关节 2 的 驱动 力 。 
关节 1 上 的 驱动 力 为 : 
scam m. 
< AE, BE, Saa. 
dt əà 30 30 


d emri tmr — 0+ (gcosOmir + gcosôm:r) 


= mir20 十 m. 0 十 2m,r Ü + gcos0(mir, + m;r) 
关节 1 是 转动 关节 ,所 以 f, 是 转 和 矩 , 即 : 
ti = (miri + mar? )0 + 2m,r r+ gcos0(mi r, + m;r) (6-3) 
在 式 (6-3) 中 ,关节 1 BJ d3K 846 kB Ej HL as A W 2 W BJ3ë sJ E at k Ei y Pe Re, Hp, 
(mir! +m.r2)0 项 与 关节 1 的 加 速度 和 关节 2 的 位 置 相关 ,2zzarzr6 项 与 关节 1 和 关节 2 的 
速度 及 关节 的 2 位置 相关 ,gcos0Cman 十 mz7) 项 与 两 关节 位 置 r+ 和 0 相关 。 
关节 2 上 的 驱动 力 为 : 
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— d ƏE, 2E, 十 3P， d 2E; 2E, | 2E, 
dt3ġ, aq I dt ə ç ər ər 


ER: (m, t)— m:rĝ? + m, gsin0 


dt 
= m, r — m;rÜ0° + m: gsin0 
关节 2 是 移动 关节 ,所 以 f, 是 力 。 关 节 2 的 驱动 力也 与 机 器 人 两 关节 的 运动 变量 及 其 


微分 皆 相 关 。 其 中 ,zz 7 项 与 连 杆 2 的 加 速度 相关 ,msr6? 项 与 关节 1 的 速度 及 关节 2 的 位 
置 相关 ,mzgsin9 项 与 关节 1 的 位 置 相 关 。 
该 RP 机 器 人 的 动力 学 模型 为 : 


j = (mr? + m;r2)0 + 2m,r r+ gcos0(miri + mr) 
(6-4) 

fi = m, r — mr? + m; gsin0 

式 (6-4) 表 示 RP 机 器 人 各 关节 上 的 驱动 力 与 各 连 杆 运动 之 间 的 关系 。 这 种 关系 是 非 
常 复杂 的 耦合 关系 ,例如 关节 1 的 驱动 力矩 不 仅 与 关节 1 的 位 置 变量 0 及 其 微分 相关 ,而且 
与 关节 2 的 位 置 变量 > 及 其 微分 相关 ; 关节 2 的 驱动 力 不 仅 与 关节 2 的 位 置 变量 r 及 其 微 
分 相关 ,而 且 与 关节 1 的 位 置 变量 0 及 其 微分 相关 。 

综 上 所 述 ,采用 Lagrange 方法 建立 机 器 人 动力 学 模型 的 基本 步骤 如 下 : 

(1) 计算 机 器 人 各 连 杆 质心 的 位 置 和 速度 ; 

(2) 计算 机 器 人 的 总 动能 ; 

(3) 计算 机 器 人 的 总 势能 ; 

(4) 构造 Lagrange M% L; 

(5) 采用 式 (6-1) 或 式 (6-2) 推 导 Lagrange 动力 学 方程 。 

上 面 介 绍 的 Lagrange 方 程 也 被 称 为 第 二 类 Lagrange 方程 , 它 仅 适用 于 用 动能 、 势 能 及 
广义 主动 力 等 标量 就 能 描述 的 质点 系统 ,而 且 只 适用 于 完整 约束 系统 ,对 于 非 完整 约束 系 
统 , 需 采用 改进 的 Lagrange 方程 。 


6.2.2 Lagrange 动力 学 方程 分 析 
补充 所 有 的 变量 项 ,扩展 6.2. 1 节 例 1 所 求 得 的 动力 学 方程 (6-4) ,并 将 系数 进行 分 类 
简化 ,可 得 Lagrange 动力 学 方程 的 一 般 形式 为 : 
fi = Dauð + Dx F+ Dn Di ? + Di £ + Dia Ò+ D. (6-5) 
fa = D.,0 + Dy, F+ D, Ë + Doz 7 + Da0 t + Don PÒ + D, (6-6) 
下 面 对 式 (6-5) 和 式 (6-6) 中 的 系数 作 简 要 介绍 : 
(1) D; 是 关节 7 的 有 效 惯量 ,Dig; 是 关节 ;的 加 速度 在 关节 上 产生 的 惯性 力 。 
(2) D, GZ j): C 0 j 对 关节 i 的 看 合 惯量 , D q, 是 关节 j 的 加 速度 在 关节 i 上 产生 
的 看 合力 。 x 
(3) D;;q 是 关节 7 的 速度 在 关节 上 产生 的 向 心力 。 
(4) Di,q;q, ,D529 是 作用 在 关节 1 上 的 科 氏 力 。 
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(5) D; 是 作用 在 关节 i 上 的 重力 。 , 

在 式 (6-5) 和 式 (6-6) 中 ,第 一 项 和 第 二 项 是 惯性 力 项 ,第 三 项 和 第 四 项 是 向 心力 项 ,第 
五 项 和 第 六 项 是 科 氏 力 项 ,最 后 一 项 为 重力 项 。 

由 于 Lagrange 动力 学 方程 一 般 包 括 惯 性 力 项 、 向 心力 项 、 科 氏 力 项 和 重力 项 四 个 组 成 
部 分 ,所 以 Lagrange 动力 学 方程 通常 会 写成 如 下 简 式 : 

r= D(q) q + h(q.q) + G(q) 

其 中 ,D(g) 表 示 质 量 和 矩阵 ,为 nXn 对 称 阵 ; h(g,9g) 表 示 离 心力 和 科 氏 力 ,为 nX1 HR BE; 
G(g) 表 示 重 力 , 为 nX1 FREE; qg 和 9g 是 状态 量 / 关 节 变 量 ,为 nX1 EE, 

对 照 式 (6-4) ,可 得 式 (6-5) 和 式 (6-6) 中 各 项 系数 ,如 表 6.1 所 示 。 


表 6.1 各 项 系数 
名 称 关节 1 e 
惯性 力 项 | Du =m ri Tm;r2 ; Di 一 0 Dza =0; Dz =m; 
向 心力 项 | Din =0; Diz =0 Dai =— mr; Dazz =0 
科 氏 力 项 | Du =m:r; Din =mr Da: =0; D; 一 0 
重力 项 Di = gcos0(mi r, 十 7227) D; = m; gsin0 


参照 表 6. 1, 对 于 图 6.2 所 示 的 RP 机 器 人 ,可 以 得 出 如 下 结论 : 

d) 关节 1 和 关节 2 之 间 没 有 耦合 惯性 力 (Dis 王 0,D: 二 0)。 

(2) 关节 1 没有 受到 向 心力 作用 (Di =0; Dizs =0), £ f 2 受到 关节 1 作用 的 向 心 
J (D, 三 一 as7)。 

(3) 关节 1 受到 科 氏 力 的 作用 ,关节 2 无 科 氏 力作 用 。 

(4) 关节 1 和 关节 2 都 受到 重力 作用 。 

(5) 有 效 惯量 对 于 转动 关节 是 转动 惯量 (Du = mri tmr), X FE aR E 
(Dz =m). 

PRH (Coriolis force) 亦 称 为 科 里 奥 利 力 或 哥 氏 力 , 是 以 法 国 物 理学 家 科 里 奥 利 
(Gaspard Gustave de Coriolis,1792 一 1843 年 ) 的 名 字 命名 的 一 种 力 。 科 氏 力 是 以 牛顿 力学 
为 基础 的 。1835 年 Coriolis 在 《物体 系统 相对 运动 方程 ) 的 论文 中 指出 ,如 果 物 体 在 匀速 转 

O 动 的 参考 系 中 作 相 对 运动 ,就 有 一 种 不 同 于 通常 离心 力 的 惯 

Yá | x 性 力作 用 于 物体 ,这 种 力 被 称 为 复合 离心 力 , 其 大 小 和 方向 可 

4 用 2mo Xw 表示 ,其 中 m 为 物体 质量 ,wv 为 相对 速度 ,w 为 参 

考 系 的 角速度 。 下 面 举 例 说 明 , 在 图 6.3 中 ,一 圆 盘 做 匀速 转 

Ce D 动 ,转速 为 w ,其 上 一 质点 相对 于 惯性 坐标 系 OX YZ 做 直线 运 

动 , 速 度 为 v , 则 该 质点 相对 于 旋转 坐标 系 的 轨迹 是 一 条 曲 

图 6.3 科 里 奥 利 力 示意 图 。” 线 。 在 旋转 坐标 系 内 ,得 有 一 个 力 驱 使 质点 运动 轨迹 形成 曲 
线 , 这 个 力 就 是 科 氏 力 。 

例 2: 假设 图 6.2 中 的 RP 机 器 人 的 实际 参数 为 : mi 二 10kg,mz 二 5kg,n7i 二 lm,r 为 


1~2m; 各 关节 的 最 大 速度 6 = 二 1rad/s,7 二 1m/s; 各 关节 的 最 大 加 速度 9 = 1rad/s’ ,六 一 
2m/s*。 计 算 下 列 情况 下 各 关节 的 驱动 力 : 
(1) 手臂 伸 至 最 长 ,静止 状态 下 ,计算 关节 1 在 垂直 和 水 平 位 置 时 的 驱动 力 ; 
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(2) 手臂 伸 至 最 长 ,以 最 大 速度 从 垂直 位 置 运 动 到 水 平 位 置 ,计算 关节 1 的 驱动 力 ; 

(3) 手臂 伸 至 最 短 ,两 关节 都 以 最 大 加 速度 启动 ,计算 垂直 和 水 平 两 种 位 置 时 关节 1 和 
关节 2 的 驱动 力 。 

解 : 

前 面 已 推导 出 关节 1 和 关节 2 的 驱动 力 公 式 , 因 此 将 每 种 情况 下 对 应 的 参数 代入 公式 
即 可 得 到 对 应 关节 的 驱动 力 。 

(1) 对 于 情况 (1) ,可 得 参数 0=0 或 = 0 =0=0,r=2,° 一 * 一 0, 将 这 些 参数 代 人 


式 (6-3) ,得 关节 1 的 驱动 力矩 为 : 


tı = gcos0O(miri + m;r) = 196cos0 (6-7) 
式 (6-7) 说 明 , 在 情况 (1) 时 , 仅 重力 项 对 关节 1 的 驱动 力 
EA. 
RP 机 器 人 所 对 应 的 水 平和 垂直 两 个 位 置 如 图 6. 4 所 示 。 ý 
水 平时 : bg 一 0,m =196kg * m° /s° OA- 
FEN: 0 一 3 和 一 0 图 6.4 RP 机 器 人 的 垂直 和 
这 说 明 关 节 1 的 驱动 力矩 变化 很 大 。 水 平 两 种 位 置 


(2) 对 于 情况 (2) ,可 得 各 参数 0:52 0,0 =1,9=0,r=2,} 二 1,7 二 0, 将 这 些 参数 代 人 
式 (6-3) ,得 关节 1 的 驱动 力矩 为 : 


ri = 2m;r rÊ + gcos0(miri + m;r) = 20 + 196cos0 
绘制 关节 1 的 驱动 力矩 变化 曲线 如 图 6.5 所 示 。 从 图 中 可 以 看 出 ,关节 1 从 竖 直 位 置 
运动 到 水 平 位 置 的 过 程 中 ,驱动 力矩 逐渐 增 大 ,从 初 值 的 20 变化 到 终 值 的 216。 从 中 也 可 
以 看 出 科 氏 力 对 关节 1 的 驱动 力矩 有 影响 ,但 没有 重力 的 影响 大 ,其 主因 是 机 器 人 关节 速度 
比较 小 。 如 果 关 节 1 的 最 大 速度 变 为 10 rad/s,m 二 200 十 196cos0, 则 科 氏 力 的 影响 就 成 为 
主要 因素 了 。 


E 120 
~ 100 


0/° 
图 6.5 关节 1 的 驱动 力矩 随 角 度 变化 情况 
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(3) 对 于 情况 (3) ,可 得 参数 0 一 0 或 者 0= 了 ,0 二 0,0 一 1，r 1 ,六 二 0, 一 2, 将 这 些 参 
数 代 入 式 (6-4) 中 关节 1、 关节 2 的 驱动 力 公式 ,得 各 关节 的 驱动 力 为 : 


n = (mri +mr )0 + gcosô(mirı + m;r) = 30 + 147cos0 


fa = m: r + ms gsin0 = 10 + 49sin0 

在 垂直 位 置 时 : z =30.f,=10 

在 水 平 位 置 时 : z =177,f,=59 

从 上 述 数 据 可 以 看 出 : 

A) 对 于 该 RP 机 器 人 来 讲 ,关节 1 的 最 大 驱动 力 远 大 于 关节 2 的 最 大 驱动 力 ,原因 是 
连 杆 2 也 是 关节 1 的 负载 。 这 也 可 以 解释 为 什么 工业 机 器 人 离 基 座 最 近 的 俯仰 关节 电机 远 
比 其 他 关节 的 电机 大 。 

(2) 重力 负载 对 关节 驱动 力 的 影响 变化 极 大 ,在 垂直 位 置 时 对 关节 驱动 力 的 影响 是 零 ， 
在 水 平 位 置 时 对 关节 驱动 力 的 影响 最 大 。 

重力 负载 对 机 器 人 控制 的 影响 很 大 ,在 实际 应 用 中 可 采用 重力 补偿 或 前 馈 补 偿 的 方法 。 
在 图 6.6(a) 中 ,机 器 人 直接 采用 重 物 来 平衡 第 一 俯仰 关节 所 受 重力 ,从 而 降低 该 关节 电机 
所 需 的 最 大 力矩 。 在 图 6.6(Cb) 中 ,机 器 人 采用 弹簧 缸 来 平衡 第 一 俯仰 关节 的 重力 。 在 这 两 
种 平衡 重力 的 方法 中 , 重 物 平衡 的 方式 用 得 比较 早 ,该 方式 结构 简单 .成 本 低 ,但 平衡 补偿 能 
力 有 限 ; 弹簧 生 补 偿 方式 目前 在 工业 机 器 人 上 普遍 采用 ,具有 结构 紧凑 、 补 偿 效果 好 等 优 
点 ,但 成 本 比重 物 补偿 方式 高 。 


(a) 重 物 补偿 外 (b) 弹 血 氏 补偿 四 
图 6.6 两 种 工业 机 器 人 重力 补偿 方式 


6.3 Newton-Euler 动力 学 方法 

Newton-Euler 动力 学 方法 是 研究 机 器 人 动力 学 问题 的 另 一 种 重要 方法 。 该 方法 需要 
用 到 刚体 的 惯性 张 量 ,下 面 先 进行 介绍 。 

6.3.1 惯性 张 量 


惯性 张 量 是 表示 刚体 相对 于 某 一 坐标 系 的 质量 分 布 的 二 阶 和 矩阵 ,是 由 表示 刚体 质量 分 
布 的 惯性 矩 和 惯性 积 组 成 。 


第 6 章 ” 机 器 人 动力 学 |P 133 


1. RIE% 

惯性 矩 也 被 称 为 面积 惯性 矩 ,是 刚体 的 质量 微 元 与 其 到 某 坐 标 轴 距 离 平方 乘积 的 积分 ， 
表示 刚体 抵抗 扭 动 .扭转 的 能 力 , 通 常 表示 截面 抗 弯曲 能 力 。 

如 图 6.7 所 示 为 均 质 刚体 , 绕 XYZ 轴 的 惯性 矩 定义 为 : 


Ë = |l G + y dy = III G taia 
v m 


a. | (2 二 pdy = III CET 
v m 


I = |l (y? + z=*)odV = | (y + z2)dm 
v m 


其 中 ,o 是 刚体 的 密度 ,dm 一 pdV 表示 体积 微 元 
图 6.7 均 质 刚体 在 坐标 系 中 的 表示 
的 质量 。 
2. 惯性 积 
惯性 积 也 被 称 为 质量 惯性 积 , 是 刚体 的 质量 微 元 与 其 两 个 直角 坐标 乘积 的 积分 总 和 。 
均 质 刚体 的 惯性 积 定义 为 : 
I,, = E = III xydm 


I, = III yzodV = | yz dm 
v m 

iri | ze di == III ri 
v m 


对 于 给 定 的 刚体 ,惯性 积 的 值 与 参考 坐标 系 的 位 置 及 方向 有 关 , 如 果 选 择 的 坐标 系 合 
适 ,可 使 惯性 积 的 值 为 零 。 当 相对 于 某 一 坐标 轴 的 惯性 积 为 零 时 ,该 坐标 轴 被 称 为 惯性 主轴 
或 主轴 (Principal Axis)。 很 显然 ,如 果 刚 体 本 身 具有 几何 对 称 性 ,那么 它 的 对 称 轴 就 是 它 
的 惯性 主轴 。 但 是 ,即使 是 完全 没有 任何 对 称 性 的 刚体 也 是 存在 惯性 主轴 的 。 

3. 惯性 张 量 

惯性 张 量 (Inertia tensor) 是 描述 刚体 作 定点 转动 时 转动 惯性 的 一 种 度量 ,描述 了 刚体 
的 质量 分 布 ,用 包含 惯性 矩 与 惯性 积 的 9 个 分 量 构成 的 对 称 和 矩阵 表示 。 以 坐标 系 {A)} 为 参 
考 系 ,刚体 相对 于 参考 系 {A)} 的 惯性 张 量 定义 为 ， 


Li E 1 
Ws |=, Io =o 
N AERES D, 


惯性 张 量 跟 坐标 系 的 选取 有 关 , 如 果 选 取 的 坐标 系 使 各 惯性 积 为 零 , 则 此 坐标 系 下 的 惯 
性 张 量 是 对 角 型 的 ,此 坐标 系 的 各 坐标 轴 被 称 为 惯性 主轴 。 
与 惯性 张 量 不 同 , 转 动 惯 量 (Moment of Inertia) 是 表示 刚体 绕 定 轴 转动 时 转动 惯性 的 


一 种 度量 。 在 经 典 力学 中 ,转动 惯量 又 称 质 量 惯 性 矩 ,用 了 = mr 表示 ,其 中 m 是 质点 的 质 
量 ,r 是 质点 到 转轴 的 距离 。 刚 体 作 定点 转动 的 力学 情况 要 比 绕 定 轴 转 动 复杂 。 


6.3.2 Newton 方程 与 Euler 方程 


任意 刚体 的 运动 可 分 解 为 质心 的 平 动 与 绕 质心 的 转动 。 质 心 的 平 动 可 用 Newton 方程 
描述 , 绕 质 心 的 转动 可 用 Euler 方程 描述 。 
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牛顿 第 二 运动 定律 指出 了 力 ,加 速度 、 质 量 三 者 之 间 的 关系 : 物体 加 速度 的 大 小 跟 作用 
力 成 正比 , 跟 物体 的 质量 成 反比 ,加 速度 的 方向 跟 作用 力 的 方向 相同 。 
如 图 6.8 所 示 , 对 于 质量 为 m 的 刚性 连 杆 , 力 Fe 作用 在 连 杆 质心 上 使 它 做 直线 运动 ， 
依据 牛顿 第 二 运动 定律 ,可 建立 如 下 力 平衡 方程 (Newton DFE): 
Fe = mvc (6-8) 
式 (16-8) 中 ,vc 是 连 杆 质心 的 线 加 速度 。 
Euler 方程 是 Euler 运动 定律 的 定量 描述 ,而 Euler 运动 定律 是 牛顿 运动 定律 的 延伸 。 
在 牛顿 运动 定律 发 表 超过 半 个 世纪 后 ,1750 年 Euler 提出 了 Euler 方程 。Euler 方 程 是 建 
立 在 角 动 量 定理 的 基础 上 描述 刚体 旋转 运动 时 所 受 外 力矩 与 角速度 、 角 加 速度 之 间 的 
关系 。 
如 图 6.9 所 示 , 对 于 绕 质 心 旋转 角速度 为 , 角 加 速度 为 w 的 刚性 连 杆 ,可 以 采用 Euler 
方程 建立 如 下 的 力矩 平衡 方程 : 
Nc = Iw + w X Iw (6-9) 
其 中 ,Nc 是 作用 在 连 杆 质心 上 的 合 外 力矩 ,5 为 连 杆 在 质心 坐标 系 {1C} 中 的 惯性 张 量 ， 
质心 坐标 系 {C} 的 原点 位 于 刚体 的 质心 。 


图 6.8 力 Fc 作用 在 刚体 质心 图 6.9 力矩 Nc 作用 在 刚体 质心 


需要 注意 的 是 ,刚体 绕 定 轴 转 动 时 ,角速度 矢量 w 和 角 加 速度 矢量 w 都 是 绕 着 固定 轴线 
的 ; 而 刚体 绕 定 点 运动 时 ,角速度 矢量 w 的 大 小 和 方向 都 在 不 断 变化 , 角 加 速度 矢量 w 的 方 
向 是 沿 着 w 的 矢量 曲线 的 切线 。 一 般 情况 下 , 角 加 速度 矢量 w 与 角速度 矢量 w 不 重合 。 

式 (6-8) 和 式 (6-9) 组 合 起 来 被 称 为 Newton-Euler 方程 , 它 是 Newton-Euler 动力 学 方 
法 的 基础 。 


6.3.3 %69 Newton-Euler 动力 学 方法 


如 果 已 知 机 器 人 关节 的 位 置 、 速 度 和 加 速度 以 及 机 器 人 的 运动 学 和 质量 分 布 信息 ,可 以 
采用 Newton-Euler 动力 学 方法 求 出 关节 需要 提供 的 驱动 力 / 力 矩 。Newton-Euler 动力 学 
方法 主要 包括 速度 和 加 速度 的 递 推 计 算 及 力 和 力矩 的 递 推 计算 两 个 步 又。 

1. 速度 和 加 速度 的 外 推 公式 

对 于 一 个 具有 个 关节 的 机 器 人 ,采用 Newton-Euler 方程 计算 作用 在 连 杆 上 的 惯性 
力 / 力 矩 , 需 要 知道 任意 时 刻 连 杆 质心 的 线 加 速度 、 绕 质心 的 角速度 和 和 角 加 速度 。 这 里 采用 
一 种 递 推 的 方式 ,从 连 杆 1 开始 向 外 递 推 , 直 到 连 杆 ,依次 计算 出 需要 的 速度 和 加 速度 。 

假设 已 知 连 杆 i 在 连 杆 坐标 系 { 引 中 的 角速度 :w;, 则 连 杆 i 十 1 在 连 杆 坐标 系 们 十 1) 中 
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的 角速度 为 : 
ne E, 关节 i 十 1 为 转动 关节 
(6-10) 
AR w; 关节 i 十 1 为 移动 关节 
其 中 ,6 是 关节 i 十 1 的 转动 速度 ,'*!2,41 是 坐标 系 人 i 十 1} 中 Z 轴 的 矢量 表达 。 
对 式 (6-10) 相 对 于 时 间 i 求 导 , 可 得 连 杆 i 十 1 在 坐标 系 {i 十 1} 中 的 角 加 速度 为 : 


a ii XË A Za Tü a Za 美 节 i 十 1 为 转动 关节 
wi 一 
aiia: 关节 i 十 1 为 移动 关节 
(6-11) 


为 了 求 连 杆 坐标 系 {i 十 1} 原 点 的 线 速度 和 线 加 速度 , 令 "p; 和 "p;41 分 别 为 坐标 系 {i} 和 
坐标 系 人 i 十 1) 的 原点 在 基 上 坐标 系 {0} 中 的 位 置 向 量 , 令 'p;+1 为 坐标 系 {i 十 1) 的 原点 在 坐标 系 
他 中 的 位 置 向 量 , 则 在 坐标 系 {0} 、{ 引 和 {i 十 1} 中 ,三 个 位 置 向 量 可 构成 如 图 6. 10 所 示 的 矢 
量 三 角形 ,表示 为 : 


0 


Pin =° Pi HRP in (6-12) 


图 6.10 三 位 置 矢量 构成 的 矢量 三 角形 


对 式 (6-12) 相 对 于 时 间 /上 求 导 ,可 得 坐标 系 1i1) 的 原点 在 基 坐 标 系 (0} 中 的 线 速 
EX: 
in =° o, HR Pii Pow; XoR'pin 


=° KA 十 及 mi Fwi; Xi RP: í (6-13) 
由 于 ; ?R= 二 ;RIR ,对 式 (6-13) 两 侧 同 乘 以 ,1R, 得 : 
ya =; R'o; + Rv ti RC'ay Xp) (6-14) 


下 面 按 关节 i 十 1 是 转动 关节 和 移动 关节 分 别 求 坐标 系 {i 十 1} 原 点 的 线 加 速度 。 
d) 当 关 节 i 十 1 为 转动 关节 时 ,有 'w;+1 二 0, 式 (6-14) 可 化 简 为 : 
Ha SR ha Ro Xp.) (6-15) 
对 式 (6-15) 相 对 于 时 间 t 求 导 , 可 得 坐标 系 (i 十 1} 原 点 的 线 加 速度 为 : 
(iq aa = RL Ho:X' Pin Ho: X Ce; Xipin)] 


(2) 当 关 节 i 十 1 为 移动 关节 时 ,有 i'wi+1 Sdi Zind EAH i41 的 移动 速度 标 
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量 , 则 式 (6-14) 化 简 为 : 


ny =R MU Xpan Yd FS a, (6-16) 
对 式 (6-16) 相 对 于 时 间 t 求 导 , 可 得 坐标 系 {i 十 1) 原 点 的 线 加 速度 为 : 


dC Za) 
dt 


+1 ` Z i EAL . ; i $ 
"oa =A RV FoX Pur |] 


=; HARU FoX pia Fæ: >x TRE AE Fwi; Wp > 十 
in ea a wii X da a 
= iR W iH oX Pi Fw; X Cwi X Piy )] 


Hia a i on SG n 
所 以 , 求 坐标 系 们 十 1} 原 点 的 线 加 速度 的 递 推 式 为 : 
ARCO CP a FoX CoiX Pia) Fe] 2638 2-1 RT 
Haa = 4 AR O: XP For X Co X Pd FV] 
FI a ba ha u h a Ey 关节 i 十 1 为 移动 关节 
(6-17) 
需要 说 明 的 是 ,应 用 上 述 递 推 公式 计算 连 杆 1 的 速度 和 加 速度 时 : w= w0. 
假设 连 杆 i 的 质心 为 C;, 以 该 质心 为 原点 建立 坐标 系 {C;} ,该 质心 坐标 系 与 连 杆 坐标 系 
{让 具有 相同 的 姿态 ,'Pc 是 质心 C, 在 连 杆 坐标 系 (让 中 的 位 置 拓 量 , 则 质心 C, ERA li) 


中 的 线 速度 为 : 
uc ='@: XPc 十 vi (6-183 
对 式 (6-18) 求 导 , 可 得 连 杆 i HIDE ERTA ER R (i) P BS 22 IE BEW : 
bc =ð; XP o Twi X Co: XP o ) +G, (6-19) 


TEKH 是 转动 关节 还 是 移动 关节 , 式 (6-19) 都 适用 。 
由 计算 出 的 连 杆 线 加 速度 .角速度 和 角 加 速度 ,可 以 通过 Newton-Euler 方程 计算 出 施 
加 在 连 杆 质 必 的 惯性 力 和 惯性 转 矩 : 
'F; = m;'Ŭ c, ， 
iN, =S Io Fo, XS Io, 
2. 力 和 力矩 的 内 推 公 式 
在 计算 出 每 个 连 杆 所 受 的 惯性 力 和 惯性 转 矩 后 ,下 一 步 计算 各 关节 需 提供 的 驱动 力 / 转 
E. HFE 6.11 所 示 的 连 杆 i, 根据 达 朗 贝尔 原理 建立 连 杆 i 的 力 平衡 方程 和 力矩 平衡 方 
程 如 下 : 


图 6.11 连 杆 i 的 受 力 分 析 ( 包 括 惯 性 力 和 惯性 力矩 ) 
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力 平衡 方程 : F= fi R 1 fi41( 不 考虑 重力 ) 

力矩 平衡 方程 : Ni 二 ni 一 niti 十 (一 :Pe X fi CPi Pe) Xfi (E i 
心 转化 ) 

这 里 ,'f; 是 连 杆 i 一 1 作用 于 连 杆 i 的 力 在 坐标 系 { 引 中 的 表达 ,'n; 是 连 杆 i 一 1 作用 于 
连 杆 i 的 力矩 在 坐标 系 {i} 中 的 表达 ,作用 点 在 坐标 系 { 引 的 原点 ; 'f ;41 是 连 杆 i 十 1 作用 于 
连 杆 i 的 力 在 坐标 系 { 店 中 的 表达 ,'n; ,1 是 连 杆 i 十 1 作用 于 连 杆 宗 的 力矩 在 坐标 系 {z} 中 的 
表达 ,作用 点 在 坐标 系 {i 十 1} 的 原点 。 

将 力 平衡 方程 代入 力矩 平衡 方程 ,并 用 旋转 和 矩阵 做 坐标 系 转化 可 将 力矩 平衡 方程 写 为 : 

Ni ='n R nn — Po. XE—P XR fin 
可 以 得 到 连 杆 i 一 1 作用 于 连 杆 i 的 力 和 力矩 的 递 推 计算 公式 : 
Si FRY fin TF, 
H =N; RT nmin +t Pe XF; HP pe a Pm 

通过 式 (6-20) 递 推 公式 ,可 以 从 机 器 人 未 端 连 杆 妹 开 始 计算 ,依次 递 推 , 直 至 机 器 人 的 
基 座 ,从 而 得 到 机 如 人 各 连 杆 对 相 邻 连 杆 施加 的 力 和 力矩 。 

如 果 关 节 i 是 转动 关节 ,关节 i 的 驱动 转 矩 为 : 

z; == A 
如 果 关 节 i 是 移动 关节 ,关节 1 的 驱动 力 为 : 
fi =: fl Z, 

3. 递 推 的 Newton-Euler 动力 学 方法 

对 递 推 的 Newton-Euler 动力 学 方法 做 个 总 结 ,该 方法 主要 包括 下 面 两 部 分 内 容 。 

(1) 外 推 计算 速度 和 加 速度 。 

从 连 杆 1 到 连 杆 n 北 推 计算 各 连 杆 的 速度 和 加 速度 ,并 由 此 计算 出 各 连 杆 所 受 的 惯性 
力 和 惯性 转 矩 ,i:0->n 一 1, 计 算 公 式 如 下 : 


+H = À) +1 
š Wi =; A R w; 0 Zizi 


(6-20) 


là, A= L R' o, HR w; KUn An S Zea 
GARE XPa H wi X Co: XPa) Ho] 关节 i 十 1 为 转动 关节 
H= 2, RI: X P Fa X Ga, PY YL 


HP qa X d. Za da Zn 关节 i 十 1 为 移动 关节 


+1 a + H1 HI H Hoas 
I Ua y = wiy 2<' Pog T wi X C wiy X' Re 9 T Q; +1 
+ ac Pl í 

“Fr = Mi UG ,i 

i+ 2e MS H £ H 

Naa SA DI U wiit iti XH la wiy 


(2) 内 推 计 算 力 和 力矩 。 
从 连 杆 n 到 连 杆 1 递 推 计算 各 连 杆 内 部 相互 作用 的 力 和 力矩 及 关节 驱动 力 和 力矩 ， 
i:n 六 1 ,计算 公 式 如 下 : 
tf, SR f. u. FF; 
| =N; TIR: ha Pe XF, Pia KERA f 
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ab: i 为 转动 关节 
SEZ i 为 移动 关节 

机 器 人 在 自由 空间 运动 时 ,机 器 人 末端 所 受 的 力 为 0, 则 : 

itia e pan 0 

机 器 人 ip 机 器 人 未 端 所 受 的 力 不 为 堆 ， 需要 求 得 对 应 的 力 和 力矩 分 
EO fra AH n, , 代 和 人 到 力 和 力矩 的 递 推 计算 式 中 。 

另外 ,如 果 需 要 考虑 机 器 人 各 连 杆 自身 重力 的 作用 ,可 令 %, 二 g, 即 将 机 器 人 基 座 所 受 
的 支承 力 等 效 为 基 座 朝 上 做 加 速度 为 g 的 直线 运动 。 这 种 处 理 方式 与 考虑 各 连 杆 重力 的 
作用 完全 等 效 。 

在 机 器 人 的 动力 学 应 用 中 ,Newton-Euler 动力 学 递 推 方法 有 两 种 不 同 的 用 法 : 数值 计 
算 方 式 和 封闭 公式 方式 。 

数值 计算 方式 可 在 已 知 连 杆 质量 .惯性 张 量 ` 质 心 矢量 `. 相 邻 连 杆 坐 标 系 转换 矩阵 等 机 
器 人 信息 时 ,利用 Newton-Euler 动力 学 递 推 方法 直接 数值 计算 出 机 器 人 实现 任意 运动 所 需 
的 关节 驱动 力 /力矩 。 

封闭 公式 方式 就 是 由 Newton-Euler 动力 学 递 推 方法 推导 出 以 关节 位 置 、 速 度 、 加 速度 
为 变量 的 关节 驱动 力 / 力 矩 的 解析 表达 式 , 这 样 就 可 以 定性 分 析 动 力学 公式 的 结构 、 不 同 动 
力学 分 项 (如 惯性 力 项 ) 对 驱动 力 / 力 抢 的 影响 。 

例 3: 2R 机 械 手 如 图 6. 12(a) 所 示 , 两 连 杆 质量 集中 在 各 自 连 杆 末端 ,采用 Newton- 
Euler 动力 学 方法 推导 该 机 械 手 封闭 公式 形式 的 动力 学 方程 。 


(a) 2R 机 械 手机 构 简 图 (b) 坐标 系 建立 
图 6.12 2R 机 械 手 


解 : 

首先 ,建立 2R 机 械 手 的 基 坐 标 系 和 连 杆 坐标 系 , 如 图 6. 12(b) 所 示 。 为 了 使 视图 简洁 ， 
这 里 省 略 各 连 杆 的 质心 坐标 系 。 

然后 ,依据 建立 的 各 坐标 系 确定 Newton-Euler 递 推 公式 中 的 运动 学 和 动力 学 参数 。 

两 连 杆 质心 在 各 自 连 杆 坐 标 系 中 的 位 置 矢 量 为 : 'Pc =l Xi 2 Po = 1, X, 

由 于 各 连 杆 的 质量 集中 于 一 点 ,因此 各 连 杆 相对 于 质心 坐标 系 的 惯性 张 量 为 : ”一 
0,” =0 

由 于 2R 机 械 手 是 在 自由 空间 中 运动 ,所 以 它 的 末端 受 力 为 0, 即 : fi 50, n50 
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由 于 基 座 是 静止 的 ,所 以 : wo 一 0,owo 王 0 
这 里 考虑 各 连 杆 的 重力 作用 ,所 以 基 座 的 线 加 速度 为 : o o = g Y, 


相 邻 连 杆 之 间 的 转换 矩阵 为 : 
Ga SS Ü Ci+l Si+ 
R= saa Ga O|, R= —saa th 
0 0 1 0 0 L 


下 面 根 据 递 推 的 Newton-Euler 动力 学 公式 分 步 做 计算 。 
(1) 外 推 计 算 各 连 杆 的 角速度 . 角 加 速度 . 线 加 速度 .惯性 力 和 惯性 转 矩 。 
连 杆 1 的 角速度 . 角 加 速度 、. 线 加 速度 .惯性 力 和 惯性 转 矩 计算 如 下 : 


C1 s lro gs 
lg I = x & “€ . sl- J 
0 @ LI LO 0 
[ gs. [9 l 0 0 l 
ja = | gc |+ j. 0 + | 0 |x N. H 
L. 2 0 Ó, Å, 0 


[ gs. & — lË gs — Liġi 
= | gci 十 Lð, F 0 = gc + 0, 
LOJ Lý 0 0 


gs. — LÖ? 

1 += ta — bg a3 

F, =m TVcl 一 m gc1 十 L101 
0 

0 

0 


0 
连 杆 2 的 角速度 、 角 加 速度 、 线 加 速度 、 惯 性 力 和 惯性 转 和 矩 计算 如 下 : 


LN: == 
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C2 s 0| | 一 L108 £512 — lcs +i: S2 
2y, 一 — g =i k . se 
v3 š, s gci 十 00， 8C 12 + l,01s, + 1,01 c; 
0 0 0 0 
0 =í (Ó, +Å)? B512 — les +I, 
"a= |l +80,) | 十 0 


+ gcu + l 01s, + Lare 


0 0 0 


8S 12 — htc; + L101s; — L, (0, + 0, )° 


8C 12 +h 01s; + li0ic 4 À (9, Fpa) 
0 


B8512 — Lc 十 二 bisz — L, (0, +å)? 


2 zè Eg 
F, = m° Gc = m 


0 
0 


(2) 内 推 计算 各 连 杆 所 受 的 力 和 力矩 。 
连 杆 2 所 受 的 力 和 力矩 为 : 
“h= tF; 


8C 12 + 01 s, +B, “t qata, Tga) 
0 


B8512 一 hita 丰 hais == (0, + 0, )° 


? n, =*P,, x m; 


8C 12 +Å s, + li01c BC0; 二 0) 


0 
0 
0 
2 gl2C12 二 mzlils ss + mzlilzcz0! +m (0, 十 9:) 
连 杆 1 所 受 的 力 和 力矩 为 : 
cz 一 S 0] |mzh s201 — mlc? + mz gs12 — ml C +å) 
1 ra £ A > 
fi $2 c2 malicz01 — mli s201 + mzge + ml (0) +02) 
0 0 


0 
— m, h? 十 migsi 


ml, Hm gc: 
0 


+ 
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ln, = 


+ 


e | 
x + mli lz s, 01 + ml, lzcz0; +m ÈO, + 0; | n 20, +m lige 


0 
| : 
. 0, —mlhl;so (Ó, +å)? + mli gs2s12 T mili l; cC; (0, +8) + mli gczciz 
(3) 因为 两 个 关节 都 是 转动 关节 ,提取 各 关节 对 应 的 'n; 向 量 的 2 轴 分 量 ,得 两 个 关节 
的 驱动 力矩 分 别 为 : 


tı =m: Kca (0, +0,) + mli la (20, + 0,) + Cm + ma) 1 0 — m lh L, s20} 
y 2m,l L, s: 0, 0, + mlz gc + Cm; 十 zz )L gc, 


ts =m:lılzc20; + ml L, ss Ó) + mal, gc + mə Ë (9, + 0,) 
上 述 两 式 是 以 关节 位 置 、 速 度 和 加 速度 为 变量 的 关节 了 驱动 力矩 表达 式 。 可 以 看 出 该 2R 
机 械 手 封闭 形式 的 动力 学 方程 是 比较 复杂 的 ,由 此 可 以 想象 6 自由 度 机 器 人 ,甚至 7 自由 度 
机 器 人 的 封闭 形式 的 动力 学 方程 会 更 复杂 。 


6.4 Kane 动力 学 方法 


Kane 动力 学 方法 也 是 研究 机 器 人 动力 学 问题 的 重要 方法 , 它 是 建立 一 般 多 自由 度 离散 
系统 动力 学 方程 的 一 种 普遍 方法 , 既 适 用 于 完整 系统 ,也 适用 于 非 完整 系统 。 


6.4.1 Kane 方法 介绍 


Thomas R. Kane 是 美国 斯 坦 福 大 学 应 用 力学 教授 ,他 于 20 世纪 60 年 代 提 出 了 分 析 
系统 动力 学 的 Kane 方法 。Kane 方法 综合 了 分 析 力 学 和 矢量 力学 的 优点 ,采用 广义 速率 作 
为 广义 坐标 的 独立 变量 ,通过 引入 偏 速度 、 偏 角速度 的 概念 ,建立 了 代数 方程 形式 的 动力 学 
方程 。 在 建立 动力 学 方程 过 程 中 不 出 现 理 想 约束 反 力 ,也 不 必 计 算 动 能 等 动力 学 函数 及 其 
导数 。 所 涉及 的 主要 概念 有 : 广义 速率 、 偏 速度 、 偏 角速度 、 广 义 主动 力 和 广义 惯性 力 。 

1. 广义 速率 

设 一 个 完整 力学 系统 S H N 个 质点 组 成 , 它 在 惯性 坐标 系 中 的 自由 度数 目 为 n, 其 位 
形 可 以 用 nn 个 广义 坐标 gq;(s 二 1,2,…,n) 来 描述 。 假 定 系统 中 第 i 个 质点 相对 惯性 系 原点 
的 矢 径 为 r;, 它 是 广义 坐标 g, 和 时 间 z 的 函数 , 即 


Ti = ri (qQ T: E D) (6-21) 
式 (6-21) 两 端 对 时 间 上 求 导 , 可 得 该 质点 速度 为 : 
Lia Yaar. L ar, I 
Q= Tr — Jg, T D (6 22) 


定义 变量 u(r 二 1,2,… n): 


u, A Dan q, + b, (6-23) 
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式 (16-23) 中 ,as 和 4 都 是 广义 坐标 9, 和 时 间 * 的 函数 ,as 和 4 的 选取 要 能 唯一 求解 出 
4 ,, 即 系数 a 构成 的 矩阵 A。 非 奇异 。w, 是 9 ,的 线性 组 合 ,类 似 广义 速度 ,但 一 般 不 可 积 ,其 
本 质 上 是 一 个 伪 速 度 ,在 Kane 方法 中 称 之 为 广义 速率 。 

2. 偏 速度 

由 式 (6-23) 定 义 的 广义 速率 ,可 得 : 


q, = J wu +y (6-24) 
r=1 


式 (16-24) 中 ,系数 zwv 所 构成 的 矩阵 W, JEN (6-16) rh RA an HTH IAE E A n BJ) E 
阵 , 即 W. 二 4x!; 系数 所 构成 的 向 量 Y, E: H Z 38 a n BJ RJ pÚ SE PE BJ 2 SE E A, 与 由 系数 0， 
所 构成 向 量 B, WR, BI Y, =A; ° B,。 

将 式 (6-24) 代 入 式 (6-15) ,可 得 : 


v = F, = > (2 iw Ju + (D) Zy, +) 


r=1 99 
上 式 可 以 改写 成 : 
p: = p, == Vrt F Ox (6-25) 
其 中 ， 
A Ar; Aar: Ər; 
w= 2 ge % = 六 可 
由 上 式 可 知 ,vw; 和 wi 都 只 是 广义 坐标 gq, 和 时 间 t 的 函数 ,与 广义 速率 无 关 。 由 式 (6-18) 
可 得 : 
— 90 - 
=a, (6-26) 


式 (6-26) 给 出 的 矢量 系数 vw; 被 称 为 第 i 个 质点 的 第 r 个 偏 速度 ,或 者 称 为 第 i 个 质点 对 
应 于 广义 速率 u, 的 偏 速度 。 偏 速度 是 一 个 矢量 , 它 的 主要 作用 是 赋予 广义 速率 方向 性 。 广 
义 速率 可 以 看 成 是 真实 速度 在 偏 速 度 上 的 投影 。 

3. 偏 角速度 

假定 一 个 系统 中 第 i 个 刚体 上 的 坐标 系 为 {O;X;Y;Z;} ,其 基 矢 量 为 ij、k, 则 该 刚体 的 
角速度 矢量 在 该 坐标 系 中 可 以 表示 为 : 


w; = wit wj wsk (6-27) 
由 矢量 的 混合 积 公 式 ,可 得 : 
JW 一 wii 一 w。(JXK) 一 大 。 Cw x j= k di 
w: =w * J = o, ° (kX iíi)= i ° (wi xk)= i $ 
— š Se — 3 di 
w; =w; ° k = w; * (X J)= j ° (wi xD=] (6-28) 


HF ij kq (s=1,2, n) A t HRR, AA : 
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中 
= 之 有 t 


daja ak 


=a 下 十 元 Jt (6-29) 


将 式 (6-24) 代 入 式 (6-29) ,可 得 : 
. n n ði n Ji 3: 
和 


r=1 s=1 Iq, 
dj v/a aW Ij 
= 2 (2 ga jet: 91 


r=1 s=1 


#= 2 a= Jet (223 2) cad 


T a + (a dy 


3 [( y+ (i Eya p+ 


aitt (a) 


— 
aa 
sie 


上 和 式 可 以 改写 成 
w == 5 wiU, F wi (6-319 
其 中 ， 


(ke e+ (E Ge + (ea) 


> 和 wi 都 只 是 广义 坐标 q, 和 时 间 z 的 函数 ,与 广义 速率 无 关 。 由 式 (6-31) 可 得 : 


9 wi 
du, 


上 式 给 出 的 矢量 系数 w; 被 称 为 第 i 个 质点 的 第 r 个 偏 角速度 ,或 者 称 为 第 i 个 质点 对 
应 于 广义 速率 ,的 偏 角速度 。 
由 式 (6-25) 可 得 ,系统 中 任 一 质点 P 相对 参考 系 {OXYZ) 运 动 的 速度 可 以 表示 成 : 


Wir 


b b= 5 vu, Fv: (6-32) 
由 式 (6-31) 可 得 ,系统 中 任 一 刚体 相对 参考 系 {OXYZ} 运 动 的 角速度 可 以 表示 成 : 
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w= X) wu, +a, (6-33) 


在 式 (6-25) 和 式 (6-26) 中 ,vw, sw, (r=1.,2,--- , N) ll v; ,都 是 广义 举 标 G=], 2,…,n) 和 
时 间 t 的 函数 。w, 称 为 质点 P 在 参考 系 {OXYZ} 中 的 第 + 个 完整 偏 速度 ,w, 称 为 刚体 在 参考 
系 {OXYZ} 中 的 第 7 个 完整 偏 角速度 。 它 们 是 速度 和 角速度 相对 某 个 广义 速率 的 偏 导数 。 

偏 速度 和 偏 角速度 是 Kane 方法 中 非常 重要 的 参数 。 广 义 速率 ,的 选取 应 当 在 满足 
式 (6-23) 中 系数 a, 构 成 矩阵 非 奇 异 的 条 件 下 ,使 偏 速度 和 偏 角速度 的 表达 式 尽量 简单 。 由 
于 广义 速率 的 选取 不 唯一 ,使 得 同一 质点 或 刚体 可 以 有 不 同形 式 的 偏 速度 和 偏 角速度 。 但 
是 对 于 质点 系 中 每 一 个 质点 和 刚体 系 中 每 一 个 刚体 ,都 分 别 有 与 系统 自由 度数 相同 数目 的 
偏 速度 和 偏 角 速度 。 因 此 ,在 求解 偏 速度 和 偏 角速度 时 ,需要 指明 是 哪个 质点 或 刚体 对 应 哪 
个 独立 速度 的 偏 速度 或 偏 角速度 。 

上 述 推导 是 针对 完整 力学 系统 的 。 对 于 非 完 整 力学 系统 ,可 以 假定 非 完整 力学 系统 在 
参考 系 {1OXYZ} 中 的 运动 能 用 ) 个 广义 速率 ww (r= 1,2,: ,n) 3678 , X £ 322 m 个 非 完 整 
约束 , 则 系统 的 自由 度数 目 为 k= 二 n 一 m。 同 时 ,可 以 假定 在 nn 个 广义 速率 uw,(r 二 1,2,*…,n) 
中 的 前 & 个 u(r 二 1,2,…,k) 是 独立 的 , 则 后 面 的 mm 个 广义 速率 u(r 二 =k 十 1,k 十 2,…,n) 可 
以 用 前 上 个 u(r 二 1,2,…,k) 表 示 为 : 


u, = Jast, +b, (6-34) 
z=] 


式 中 ,a 和 6, 都 是 广义 坐标 gq, 和 时 间 z 的 函数 。 
式 (6-34) 是 系统 的 非 完 整 约束 方程 。 非 完整 系统 中 的 任 一 质点 P 相对 参考 系 {OXYZ) 
运动 的 速度 及 任 一 刚体 相对 参考 系 {OXYZ} 运 动 的 角速度 分 别 可 以 表示 为 : 


m—m 


4. 广义 主动 力 
广义 主动 力 是 指 机 器 人 系统 上 每 一 个 质点 上 作用 的 主动 力 F 与 该 点 对 应 于 广义 速率 
u, 的 偏 速度 v;, 的 标量 积 之 和 ,可 以 用 K, 表示 , 即 


J = Èr . D, (6-39) 


对 于 刚体 ,广义 主动 力 可 表述 为 : 作用 于 刚体 简化 中 心 上 的 主 矢 和 主 和 矩 分 别 与 该 点 对 
应 于 某 一 广义 速率 的 偏 速度 与 偏 角 速度 的 标量 积 之 和 , 称 为 刚体 对 应 于 该 广义 速率 的 广义 
主动 力 。 假 设 O 点 为 刚体 的 简化 中 心 , 则 有 
K, = Fo * vor + Mo ° w, (6-40) 
证 明 如 下 : 
当 刚 体 作 一 般 运动 时 ,其 上 任 一 质点 i 的 速度 为 : 
v: = vo + w X To 


式 中 ,vo 表示 简化 中 心 的 速度 ,w 表示 刚体 的 角速度 ,ri 表示 质点 i 相对 简化 中 心 O 点 
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的 矢 径 。 
若 将 vo 和 w 都 用 广义 速率 表示 可 以 得 到 
Ur = Vor t œ, X rio (6-41) 
sK(6-41) B] , WK F 25 i NEARE r 个 独立 速度 的 偏 速度 ,可 用 刚体 简化 中 心 O 
点 和 刚体 相对 于 第 x 个 独立 速度 的 偏 速度 和 偏 角速度 表示 。 将 式 (6-41) 代 入 广义 主动 力 的 
表达 式 , 则 有 


K,= >F e: — >F. ° (vo + o, X rio ) = >F “Uo F >F, ° w, X Tio 


es ( >F.) š Qa, SlP [ Ska x F.) w, 
证 毕 。 
5. 广义 惯性 力 
广义 惯性 力 是 指 系统 中 每 一 个 质点 的 惯性 力 Fi = —ma, 与 该 点 对 应 于 广义 速率 ww 的 
偏 速度 vw, 的 标量 积 之 和 ,可 以 用 天” 表示, 即 
K; = Sv ° Vi (6-42) 


对 于 刚体 ,与 计算 广义 主动 力 的 推导 方法 相同 ,得 


= 一 ( Zyma.) ° Vor 一 >) (rio X mai) ° w, (6-43) 
i=l i=] 
若 将 简化 中 心 选 在 刚体 的 质心 C E , IJ 
Uo. = Wc, 
= (6-44) 
> ma; = Mac 
i=] 


式 中 ,M 为 刚体 的 质量 ,ac 为 质心 的 加 速度 。 
Kane 方程 可 以 描述 为 : 对 应 于 每 一 个 广义 速率 的 广义 主动 力 与 广义 惯性 力 之 和 等 于 
零 , 即 
K,+ K; = 0 (6-45) 
对 于 完整 系统 ,Kane 方 程 与 第 二 类 Lagrange 方程 是 等 价 的 。 


6.4.2 Kane 动力 学 方法 应 用 举例 


下 面 将 举例 说 明 Kane 动力 学 方法 的 应 用 。 

例 4: 如 图 6.13 所 示 是 一 个 双 摆 机 构 ,由 两 个 杆 组 成 , 杆 长 
分 别 为 5 和 7;, 杆 件 与 竖 直 方向 的 夹 角 分 别 为 m 和 e k 
机 构 的 偏 速度 。 

取 p 和 gs 为 广义 坐标 , 则 pi 和 gz 均 为 广义 速率 。 作 单 图 6.13” 双 摆 机 构 


146 ll 机 器 人 学 一 机构、 运动 学 、 动 力学 及 运动 规划 


位 矢量 e, 和 e, 分 别 垂直 于 OA MAB, WES A 和 B 的 速度 可 分 别 表示 为 : 
va = hoe i £ 
Us = va + vas = h pie) + L; Qe; 
质点 A HRF 2, 和 gs 的 偏 速度 为 


Uag, = le . Uag, 二 一 0 
质点 B XT T e, 和 o 的 偏 速度 为 
UB, = lie . UBs, n lzez 


考虑 到 e =cospii— singi j 和 es 二 cosgzi 一 singzj ,其 中 ,i 和 j 分 别 为 X 和 YY 方向 的 单 
位 矢量 ,因此 ,可 以 将 vi 和 ws 用 (i,j) 表 示 为 
va = lige = ligi(cosgii— singi j) 
vp = ligiet L, @ e; = lig(cosgi— singj)+t lp Ccosœi— sine; j) 
因此 ,质点 A 和 B 各 自 相 对 于 g! 和 9; 的 偏 速度 可 以 写 为 
va, = h Ccospii — singij), vap, = 0 
ti = li(cosgii— singi j), Vas, = l: (cosi — sing: j) 
BJ 5: 图 6.14 所 示 是 一 个 质量 为 m, 半 径 为 r 的 均 质 半 圆 盘 ,在 粗糙 的 水 平面 上 摆动 ， 
设 OC=e,C 为 半圆 盘 的 质心 。 试 用 Kane 方程 求 半圆 盘 摆 动 的 微分 方程 。 


解 : 
该 系统 具有 一 个 自由 度 , 取 9 为 广义 坐标 , 令 广义 速率 x 一 0 。 
质心 C 的 速度 为 : 


ve = vo voc = ri + e0j' = (rit+ej’)0 
质心 C 对 应 于 6 的 偏 速度 为 : 


. . 
vci —ri-+ ej 


图 6. 14 半 图 盘 半圆 盘 转动 的 角速度 为 
由 一 一 OK 
半圆 盘 对 应 于 0 的 偏 角 速度 : 
w; ==—k 
在 半圆 盘 上 主动 力 只 有 其 重力 作用: 
F=—mej 
则 系统 的 广义 主动 力 为 : 
K = F + vcj =— mgj + (ri + ej”) =— mgesing 
质心 C 的 加 速度 为 : 
ae = < 一 人 [Cr 十 gj 的 = r0ij+ e0j'j eÓ(@ X j”) = rBi+ e0j' + 602 i 
则 半圆 盘 上 各 点 惯性 力 的 主 矢 为 : 


Fe= —mac= —m(r0i+eB0 j+ eÓ 2y 
半圆 盘 上 各 点 惯性 力 对 C 点 的 主 矩 为 : 


Mc=—Jca =—Je(—8k)=Je@k 
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则 系统 的 广义 惯性 力 为 : 
K * = Fc ° Ucå + Mc ° a =— mac * Vos + Jegk ° wå 
=—m(r0i+ e0 j” + e02 U) Cri + ej”) 十 Je pk:t 


=— m(r20 + ë 0 — 2reĝ cosh + reĝ? sing) 一 Je 0 
由 Kane 方程 K+ K' 一 0 ,得 : 


— mgesin0 一 m(20 + e° 0 — 2re0 cos0 + re? sint) — Je 0 = 0 
根据 平行 轴 定 理 可 得 : 


Je = Jo — me’ = imr — me’ 
因此 ,半圆 盘 摆 动 的 微分 方程 为 : 


2r (Tr => ecosg)g + reĝ’ sinf + gesin = 0 


6.5 人 小 结 


本 章 主要 针对 机 器 人 的 动力 学 建 模 问题 ,介绍 了 三 种 典型 的 方法 : Lagrange 动力 学 方 
法 、Newton-Euler 动力 学 方法 Kane 动力 学 方法 。Lagrange 动力 学 方法 和 Newton-Euler 
动力 学 方法 只 适用 于 完整 系统 ,而 Kane 动力 学 方法 既 适 用 于 完整 系统 也 适用 于 非 完 整 系 
统 。 三 种 方法 的 优 缺 点 如 下 。 

Lagrange 动力 学 方法 能 以 最 简单 的 形式 求 得 复杂 系统 的 动力 学 方程 ,具有 显 式 结构 ， 
但 计算 效率 比较 低 。 即 Lagrange 动力 学 方法 推导 容易 ,计算 复杂 。 

Newton-Euler 动力 学 方法 的 计算 速度 快 ,能 够 满足 伺服 系统 的 响应 速度 ,便于 实时 控 
制 ,但 方程 式 中 含有 相 邻 杆 件 之 间 的 约束 力 ,推导 复杂 。 即 Newton-Euler 动力 学 方法 推导 
复杂 ,但 计算 简单 。 

Kane 动力 学 方法 兼 有 矢量 力学 (Newton-Euler 动力 学 方法 ) 和 分 析 力 学 (Lagrange 动力 
学 方法 ) 的 特点 ,在 建立 动力 学 方程 过 程 中 不 出 现 理 想 约束 反 力 ,也 不 必 计 算 动能 等 动力 学 函 
数 及 其 导数 ,只 需 进行 矢量 的 点 积 . 又 积 运算 而 不 需要 求 导 ,计算 效 率 较 高 ,便于 计算 机 实现 。 
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机 器 人 的 运动 规划 


CHAPTER 7 


机 器 人 在 实际 作业 过 程 中 的 运动 是 规划 好 的 , 既 可 以 是 离线 规划 的 ,也 可 以 是 在 线 规划 
的 ,机 器 人 作为 执行 机 构 只 是 在 实现 规划 的 运动 。 机 器 人 的 运动 规划 (Motion Planning) 包 
括 路 径 规 划 (Path Planning) 轨迹 规划 (CTrajectory Planning) , 

路 径 规划 就 是 在 给 定 起 点 位 置 和 终点 位 置 的 条 件 下 规划 出 满足 某 种 约束 条 件 的 机 器 人 
运动 路 径 ,比如 最 短路 径 .无 碰撞 路 径 等 。 这 里 的 路 径 是 不 含 时 间 变 量 的 机 器 人 位 置 曲 线 ， 
如 路 径 被 描述 为 f 王 PCz,y,z)。 著 名 的 旅行 商 问 题 CTraveling Salesman Problem, TSP) $È 
是 一 个 经 典 的 路 径 规划 问题 ,该 问题 只 是 寻找 一 条 最 短路 径 , 且 该 路 径 是 不 包含 时 间 .速度 、 
加 速度 等 变量 的 。 

轨迹 规划 就 是 根据 作业 任务 要 求 计算 出 满足 约束 条 件 的 机 器 人 运动 轨迹 。 所 谓 的 轨迹 
是 包含 时 间 变 量 的 机 器 人 运动 曲线 ,机 器 人 在 运动 轨迹 上 受到 位 置 、 速 度 、 加 速度 及 时 间 变 
量 的 约束 。 很 显然 ,机 器 人 的 运动 轨迹 比 路 径 具 有 更 多 的 约束 变量 ,也 更 具体 。 

本 章 将 对 机 器 人 的 路 径 规 划 和 轨迹 规划 做 介绍 。 


7.2 机 器 人 的 路 径 规划 


机 器 人 的 路 径 规划 一 般 是 在 机 器 人 的 操作 空间 中 进行 的 ,例如 在 给 定 环境 中 为 移动 机 
器 人 或 机 器 人 操作 臂 规划 出 无 碰撞 的 安全 路 径 。 根据 可 利用 的 环境 信息 完备 性 的 不 同 , 机 
器 人 的 路 径 规 划 又 分 为 全 局 路 径 规 划 和 局 部 路 径 规划 。 全 局 路 径 规 划一 般 是 基于 静态 环境 
的 全 局 信息 规划 出 静态 的 全 局 安全 路 径 , 全 局 路 径 规划 一 般 也 称 为 离线 规划 或 静态 规划 。 
局 部 路 径 规划 一 般 是 基于 机 器 人 周围 的 局 部 环境 地 图 并 结合 传感器 实时 采集 的 信息 规划 出 
机 器 人 的 动态 局 部 安全 路 径 ,局 部 路 径 规 划 又 被 称 为 在 线 规划 或 动态 规划 。 如 果 机 器 人 要 
在 大 的 空间 范围 中 运动 ,一 般 需 要 采用 全 局 路 径 规划 与 局 部 路 径 规划 结合 的 方式 。 

机 器 人 的 路 径 规划 主要 包括 环境 建 模 、 路 径 搜索 .路 径 平 滑 三 部 分 内 容 。 环 境 建 模 的 目 
的 是 建立 一 个 计算 机 进行 路 径 规划 计算 能 够 使 用 的 数字 化 环境 模型 ,即将 实际 环境 的 物理 
空间 抽象 表示 成 算法 能 够 处 理 的 数字 模型 空间 。 路 径 搜索 是 在 环境 模型 的 基础 上 应 用 搜索 
算法 寻找 出 一 条 可 行路 径 ,并 使 某 种 性 能 函数 取得 最 优 值 。- 有 时 通过 相应 算法 搜索 出 的 路 
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径 并 不 一 定 是 机 器 人 可 以 行走 的 路 径 , 因 此 需要 对 路 径 进行 处 理 与 平滑 。 
下 面 对 全 局 路 径 规划 与 局 部 路 径 规划 进行 介绍 。 


7.2.1 全 局 路 径 规划 


机 器 人 的 全 局 路 径 规划 方法 主要 有 栅 格 法 、C 空间 法 、Voronoi 图 法 .拓扑 法 和 概率 路 
径 图 法 等 。 下 面 对 栅 格 法 进行 介绍 。 

栅 格 法 是 目前 研究 和 应 用 非常 广泛 的 一 种 移动 机 器 人 路 径 规划 方法 。1968 年 ,美国 加 
州 大 学 圣地 亚 哥 分 校 的 William E. Howden 教授 提出 了 该 方法 ,但 也 有 人 认为 是 Carnegie 
Mellon 大 学 的 研究 者 最 先 提出 的 。 栅 格 法 实际 上 是 一 种 环境 建 模 方法 , 它 将 环境 空间 分 解 
成 一 系列 的 单元 ( 栅 格 ) ,并 把 这 些 单元 用 满 , 空 各 混合 标记 : 如 果 一 个 单元 完全 被 障碍 物 
占据 , 则 标记 为 满 ; 如 果 单 元 内 没有 障碍 物 , 则 标记 为 空 , 如 果 单 元 内 部 分 被 障碍 物 占 据 ， 
则 标记 为 混合 。 这 样 就 可 以 建立 环境 的 栅 格 地 图 ,也 就 是 将 机 器 人 运动 的 环境 用 格子 图 
来 表示 。 图 7. 1(a) 所 示 的 环境 中 有 一 个 障碍 物 , 采 用 栅 格 法 将 整个 环境 用 一 个 6X8 的 
栅 格 表示 ,对 于 栅 格 中 有 障碍 物 的 表示 为 满 ,无 障碍 物 的 表示 为 空 , 建 立 的 环境 栅 格 图 如 
图 7.1(b) 所 示 。 


(a) 有 障碍 物 环境 (b) 栅 格 地 图 
图 7.1 环境 栅 格 地 图 


栅 格 的 尺寸 可 大 可 小 ,由 此 可 建立 不 同 精确 度 的 棚 格 地 图 。 如 果 栅 格 尺寸 过 大 , 则 环境 
地 图 的 分 辨 率 低 , 环 境 信息 含量 少 , 会 直接 影响 路 径 规划 的 效果 ,其 至 无 法 找到 最 优 路 径 或 
可 行路 径 。 如 果 栅 格 尺寸 过 小 , 则 环境 地 图 的 分 辨 率 很 高 ,环境 信息 很 充分 ,但 需要 大 量 的 
信息 存储 空间 ,导致 路 径 规划 的 时 间 长 。 

栅 格 地 图 可 用 四 又 树 (Quadtree) 和 八 叉 树 (Octree) 来 表示 。 四 叉 树 是 将 障碍 和 自由 空 
间 的 位 置信 息 存储 在 一 个 分 级 结构 中 的 环境 建 模 方法 ,整个 工作 空间 用 一 个 根 节点 表示 ,每 
个 根 节点 有 四 个 子 节点 。 每 个 子 节点 对 应 着 工作 空间 中 的 一 块 子 区 域 , 并 根据 对 应 的 区 域 
和 障碍 区 域 之 间 的 关系 来 标记 它 ,如 果 节 点 对 应 区 域 位 于 障碍 的 外 边 , 则 被 标记 为 空 ; 如 果 
对 应 区 域 在 障碍 里 面 , 则 被 标记 为 满 ; 否则 ,被 标记 为 混合 。 混 合 节点 既 可 被 标记 为 满 来 处 
理 ,也 可 以 继续 分 为 四 个 子 节点 ,采用 相同 方法 进行 标记 ,直到 满足 精度 要 求 为 止 。 八 叉 树 
是 首先 将 环境 空间 分 成 八 个 相等 的 小 立方 体 或 八 分 圆 ,然后 根据 每 个 节点 是 空 、 满 ,混合 进 
行 循环 分 解 ,直到 满足 精度 要 求 为 止 。 因 此 , 八 叉 树 中 每 一 个 非 叶子 的 节点 都 有 八 个 子 
节点 。 

基于 建立 的 栅 格 地 图 可 采用 路 径 搜索 算法 ,在 地 图 上 搜索 出 一 条 从 起 始 栅 格 到 目标 栅 
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格 的 无 碰撞 最 优 路 径 , 由 此 实现 机 器 人 的 全 局 路 径 规 划 。 由 于 栅 格 地 图 的 一 致 性 和 规范 性 
好 , 栅 格 空间 中 的 邻接 关系 表达 简单 ,搜索 工作 可 用 A 算法 、Dijkstra 动态 规划 算法 .深度 
优先 法 、 宽 度 优先 法 等 来 完成 。 其 中 ,A“ 算 法 是 最 成 熟 的 搜索 算法 ,尽管 它 不 能 解决 “指数 
爆炸 ”问题 。 下 面 对 A 算法 进行 介绍 。 

A 算法 是 由 Stanford 大 学 的 Peter Hart, Nils Nilsson 和 Bertram Raphael F 1968 年 
首次 提出 ,由 人 工 智能 研究 员 Nils Nilsson 改进 后 用 于 机 器 人 的 路 径 规 划 , 之 后 Bertram 
Raphael 在 此 基础 上 进一步 改进 了 该 算法 ,并 由 Peter Hart, Nils Nilsson 和 Bertram 
Raphael 证 明 其 在 特定 条 件 下 的 最 优 性 。 

A 算法 结合 了 启发 式 搜索 方法 (这 种 方法 通过 充分 利用 地 图 给 出 的 信息 来 动态 地 做 出 
决定 而 使 搜索 次 数 大 大 降低 ) 和 形式 化 方法 (这 种 方法 通过 数学 的 形式 分 析 , 如 Dijkstra 算 
法 ) ,通过 一 个 估价 函数 f 来 估计 图 中 当前 点 到 终点 的 距离 ( 带 权 值 ), 并 由 此 决定 它 的 搜索 
方向 , 当 这 条 路 径 失败 时 , 它 会 尝试 其 他 路 径 。 具 体 算法 通过 下 面 的 例子 来 说 明 。 

如 图 7.2 所 示 ,搜索 区 域 ( 环 境 ) 被 划分 成 正方 形 网 格 ( 也 可 以 划分 成 其 他 任意 形状 的 网 
格 ), 机 器 人 要 从 起 始点 移动 到 一 墙 之 隔 的 终点 ,图 中 标 有 S 的 栅 格 为 起 始点 , 标 有 了 ñu 
格 为 终止 点 ,三 个 标 有 萎 形 网 格 的 栅 格 为 障碍 物 。 


图 7.2 机 器 人 轨迹 规划 任务 


基于 上 述 环 境 栅 格 地 图 ,把 搜索 区 域 简化 成 一 个 二 维 数组 。 数 组 的 每 一 个 元 素 是 一 个 
栅 格 ,被 标记 为 可 通过 的 (无 障碍 ) 和 不 可 通过 的 (有 障碍 )。 路 径 被 描述 为 从 起 始 栅 格 到 终 
止 机 格 经 过 的 所 有 栅 格 的 集合 。 一 旦 路 径 被 找到 ,机 器 人 就 可 以 从 一 个 栅 格 的 中 心 走向 另 
一 个 ,直到 到 达 目 的 地 。 这 些 中 点 被 称 为 “节点 ”。 

在 A 搜索 算法 中 ,通过 从 起 始 节 点 开始 ,检查 相 邻 栅 格 的 方式 ,向 外 扩展 直到 找到 目 
标 。 对 当前 处 理 节点 有 以 下 几 个 操作 需要 说 明 。 

(1) 在 搜索 路 径 的 过 程 中 ,需要 建立 两 个 列表 :“ 开 启 列表 ”和 “关闭 列表 ”。 开 启 列表 
中 存储 着 所 有 可 能 出 现在 最 终 路 径 中 的 节点 ,一 个 节点 存在 于 开启 列表 意味 着 需要 对 该 节 
点 进行 检查 分 析 ; 关闭 列表 中 保存 着 已 经 检查 过 一 次 并 且 不 需要 再 次 检测 的 节点 ,最 终 路 
径 在 关闭 列表 中 产生 。 
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(2) 对 于 一 个 待 检查 节点 ,要 关注 五 个 信息 。Q@H: 从 起 点 沿 着 产生 的 路 径 移 动 到 该 待 
检查 节点 的 移动 耗费 ; DG: 从 该 节点 移动 到 终点 的 预 估 移动 耗费 ; OF: 从 起 始点 经 过 该 
节点 到 终止 点 的 预测 耗费 ; @ 是 否 存 在 关闭 列表 (右上 角 是 否 有 a 标志); @@ 其 父 节 点 的 位 
置 (正中 心 箭头 指向 的 临近 节点 ), 如 图 7. 3 所 示 。 

对 于 G、 互 下 ,明显 有 下 一 G 十 五。 

其 中 G 的 值 可 以 精确 算出 ,但 是 H 的 值 是 通过 估计 的 方 
式 得 到 的 ,这 是 启发 式 的 ,可 以 有 多 种 方式 实现 ,本 节 只 提供 
一 种 方法 , 即 曼 哈 顿 方法 , 它 计算 从 当前 节点 到 终止 节点 之 间 

图 7.3 某 一 待 检测 节点 的 “水 平和 垂直 的 移动 耗费 总 和 (忽略 对 角 线 方向 ) 。 
数据 情况 正如 前 面 所 说 ,G 表示 沿路 径 从 起 始 节点 到 当前 节点 的 
移动 耗费 。 在 这 个 例子 里 , 令 水 平 或 者 垂直 移动 单位 节点 的 
耗费 为 10, 对 角 线 方向 耗费 为 14。 取 这 些 值 是 因为 沿 对 角 线 的 距离 与 沿 水 平 或 垂直 移动 耗 


费 的 比例 为 V2 ,或 者 说 1. 414。 为 了 简化 ,可 以 用 10 和 14 近似, 这样 做 是 考虑 下 本 身 就 是 
图 7.4 中 ,正中 心机 格 是 起 始 节 点 , 它 右 上 角 有 一 个 a 标志 ， 
64 50 44 
a 14 50|10 40|14 30 
能 通过 ) ,加 入 开启 列表 ,为 所 有 这 些 栅 格 保存 A 点 作为 “ 父 栅 格 ”。 
(5) 检查 所 有 相 邻 节点 , 跳 过 那些 已 经 在 关闭 列表 中 的 或 者 不 可 通过 的 节点 (障碍 物 )， 
具体 到 本 例 中 ,下 值 最 小 的 节点 为 起 始点 右 侧 相 邻 节点 ,将 其 从 开启 列表 中 删除 , 然 


一 个 估计 值 ,精确 度 不 高 ,这 样 可 以 在 满足 精度 要 求 的 前 提 下 大 幅 提 高 运算 效率 。 相 应 的 
表示 它 被 加 入 到 关闭 列表 中 了 。 当 前 关注 节点 的 所 有 相 邻 
下 面 介绍 A 算法 的 具体 实现 过 程 ,首先 将 起 始点 作为 ”图 7 4 待 处 理 节 点 (起 始点 ) 
(3) 从 开启 列表 中 删除 A 点 ,把 它 加 入 到 “关闭 列表 ”。 
把 它们 添加 进 开启 列表 ,对 于 新 加 入 开启 列表 中 的 节点 ,把 选中 的 栅 格 作为 其 父 节点 。 
后 将 其 添加 到 关闭 列表 中 ,观察 其 相 邻 节点 ,发 现 其 所 有 相 邻 节点 都 已 经 在 开启 列表 中 


S 
w 
S 


H 为 从 当前 节点 到 目的 节点 之 间 水 平和 垂直 栅 格 的 数量 总 和 乘 以 10。 最 后 将 G # H 相 加 
得 到 下 。 
(3) 当前 关注 节点 ,应 该 形成 如 图 7. 4 所 示 的 结构 。 在 。 a Ts Ta 
` | 4 
14 50|10 40|14 30 
50 40 
节点 都 要 检查 分 析 , 因 此 其 所 有 相 邻 节点 都 加 入 了 “开启 列 = 
表 ”, 该 节点 也 因此 成 为 其 所 有 相 邻 节点 的 “ 父 节点 ”, 同 时 ， 10 4 
作为 其 相 邻 节点 的 每 个 栅 格 都 有 一 个 指针 反 指 他 们 的 父 
当前 关注 节点 A 进行 如 下 操作 : 周围 节点 关系 图 
(1) 将 A 点 作为 待 处 理 点 存 人 一 个 “开启 列表 ”。 
(2) 寻找 A 点 周围 所 有 可 到 达 或 者 可 通过 的 栅 格 ( 跳 过 表示 障碍 物 的 栅 格 ,因为 它们 不 
执行 完 (1) 一 (3) 步 后 ,结果 如 图 7. 5 所 示 , 其 中 起 始点 的 关闭 标志 可 有 可 无 。 
(4) 继续 搜索 ,选中 当前 开启 列表 中 下 值 最 小 的 节点 ,将 其 从 开启 列表 中 删除 ,然后 添 
加 到 关闭 列表 中 。 
(6) 如 果 某 个 相 邻 节 点 已 经 在 开启 列表 里 了 ,检查 现在 的 这 条 路 径 是 否 更 好 。 换 名 
话说 ,检查 如 果 用 新 的 路 径 到 达 它 的 话 ,G 值 是 否 会 更 低 一 些 。 如 果 不 是 , 那 就 什么 都 
不 做 。 2 
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图 7.5 对 起 始点 执行 完 (1) 一 (3) 步 后 的 结果 


了 ,而 且 通 过 该 节点 到 这 些 节 点 G 值 没有 降低 ,因此 不 用 进行 其 他 操作 。 结 果 如 图 7.6 
所 示 。 


图 7.6 对 本 例 执行 一 次 (4) 一 (6) 操 作 步 又 的 结果 


下 面 仍然 要 在 开启 列表 中 选择 一 个 下 值 最 小 的 节点 ,但 是 巧合 的 是 ,在 开启 列表 中 有 
两 个 节点 的 值 并 列 最 小 是 44, 对 于 这 种 情况 可 以 选择 后 加 入 开启 列表 的 节点 进行 操作 ,这 
样 更 为 快捷 。 结 果 如 图 7.7 所 示 。 

不 断 迭 代 执 行 (4) 一 (6) 步 又 ,直到 终止 节点 加 入 开启 列表 时 迭代 结束 ,如 图 7. 8 所 示 。 

在 迭代 到 目标 节点 后 ,可 以 从 目标 节点 开始 ,在 关闭 列表 中 通过 父 节点 依次 寻找 ,这 会 
引导 回 到 初始 节点 ,这 就 是 要 找 的 路 径 , 如 图 7.9 所 示 。 

值得 注意 的 是 ,A* 算法 是 目前 为 止 最 快 的 一 种 计算 最 短路 径 的 算法 ,但 它 是 一 种 “ 较 
优 ” 算 法 , 即 它 一 般 只 能 找到 较 优 解 ,而 非 最 优 解 。 
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图 7.7 再 次 执行 (4) 一 (6) 步 又 的 结果 
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图 7.8 到 终止 节点 加 入 开启 列表 
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图 7.9 路 径 搜 索 结果 (黑色 圆 点 所 在 的 栅 格 为 路 径 ) 
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7.2.2 局 部 路 径 规 划 


` 


局 部 路 径 规划 方法 主要 有 人 工 势 场 法 、 模 糊 逻 辑 算法 .遗传 算法 和 基于 神经 网 络 的 方法 
等 。 其 中 ,人 工 势 场 法 是 比较 有 代表 性 的 方法 。 

人 工 势 场 法 (Artificial Potential Field) 是 由 斯 坦 福 大 学 的 Oussama Khatib 教授 于 
1986 年 提出 的 一 种 路 径 规 划 方 法 。 人 工 势 场 法 的 基本 思想 是 将 机 器 人 在 障碍 环境 中 的 运 
动 设计 成 一 种 在 人 造 磁 力 场 中 的 运动 ,目标 点 对 机 器 人 产生 “引力 ”, 引 导 机 器 人 向 其 运动 ， 
每 个 障碍 物 对 机 器 人 产生 “ 斥 力 ”, 避 免 机 器 人 与 之 发 生 碰撞 ,而 且 斥 力 的 大 小 跟 机 器 人 与 目 
标点 或 障碍 物 之 间 的 距离 相关 ,距离 越 近 , 斥 力 越 大 ; 距离 越 远 , 斥 力 越 小 。 机 器 人 在 路 径 


图 7.10 机 器 人 运动 环境 
常用 的 引力 场 可 表示 为 : 


上 每 一 点 所 受 的 合力 等 于 机 器 人 在 这 一 点 所 受 的 斥 力 
和 引力 的 和 , 沿 着 势 场 的 负 梯 度 方向 搜索 即 可 规划 出 
机 器 人 的 无 碰撞 安全 路 径 。 人 工 势 场 法 的 优点 是 算法 
简单 ,便于 机 器 人 的 实时 控制 ,在 移动 机 器 人 和 机 械 臂 
的 实时 避 障 和 平滑 的 轨迹 控制 方面 得 到 了 广泛 应 用 。 
人 工 势 场 法 的 缺点 是 存在 局 部 最 优 解 ,容易 陷入 局 部 
最 小 而 无 法 逃脱 的 状况 ,导致 规划 失败 。 

如 图 7. 10 所 示 是 一 个 移动 机 器 人 的 运动 环境 ,机 
器 人 的 起 点 为 S, 目 标点 (终点 ) 为 D, 在 这 个 环境 中 有 
三 个 障碍 。 下 面 介绍 如 何 采 用 人 工 势 场 法 规划 机 器 人 
的 路 径 。 这 里 忽略 机 器 人 的 尺寸 ,假设 它 为 一 个 点 。 

人 工 势 场 法 的 关键 是 如 何 构建 引力 场 和 斥 力 场 。 


Ua.(g) = >o Cas qgoal) 
其 中 ,& 是 尺度 因子 ,oC(q,qson) 表 示 当 前 机 器 人 与 目标 的 距离 。 


利用 引力 场 公式 计算 环境 空间 中 的 任意 点 与 目标 点 的 引力 场 数值 ,用 MATLAB 绘制 
出 整个 环境 的 引力 场 ,如 图 7.11 所 示 。 纵 坐标 轴 表 示 引 力 场 值 ,其 他 两 坐标 轴 表 示 点 的 坐 


引力 场 值 


图 7.11 机 器 人 运动 环境 的 引力 场 
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标 。 从 图 中 可 以 看 出 , 越 接近 目标 点 ,引力 场 值 越 小 ; 越 远 离 目 标点 ,引力 场 值 越 大 。 
机 器 人 所 受到 的 引力 为 引力 场 在 该 点 的 负 梯 度 , 即 : 
F.. (q) =— VU,, (q) =— ép(q, qra) * VOCs qeoal) 
机 器 人 所 受到 的 引力 大 小 是 跟 机 器 人 与 目标 点 之 间 的 距离 正 相关 的 。 
相 类 似 的 , 斥 力 场 可 表示 为 : 


1 


1 1 x2 
{obs =< 0 
AIET; Gi plq dol J =< 0 


U,,, (q) = 
0 (qdsqobs) > po 
其 中 ,ny 是 斥 力 尺度 因子 ,p(gq,qows) 代 表 物 体 和 障碍 物 之 间 的 距离 ,p。 代表 每 个 障碍 物 
的 影响 半径 , 即 机 器 人 离开 障碍 物 相应 的 距离 ,障碍 物 就 对 物体 没有 斥 力 影响 。 
利用 斥 力 场 公式 计算 环境 空间 中 的 任意 点 与 三 个 障碍 物 之 间 的 斥 力 场 数值 ,用 
MATLAB 绘制 出 整个 环境 的 斥 力 场 如 图 7. 12 所 示 , 纵 坐标 轴 表 示 斥 力 场 值 ,其 他 两 坐标 
轴 表 示 点 的 坐标 。 从 图 中 可 以 看 出 , 越 接近 于 障碍 物 , 斥 力 场 值 越 大 ; 离 障 碍 物 越 远 , 斥 力 
场 值 越 小 ; 超过 影响 半径 时 , 斥 力 场 值 变 为 零 。 


斥 力 场 值 


图 7.12 机 器 人 运动 环境 的 斥 力 场 


机 器 人 所 受到 的 斥 力 为 斥 力 场 的 负 梯 度 , 即 : 
1 1 
Fe (q) =— VU e (q) = | rm po Prad 
0 pl(q»qobs) > po 
机 器 人 所 受到 的 斥 力 大 小 是 跟 机 器 人 与 障碍 物 之 间 的 距离 负 相 关 的 。 
在 工作 空间 中 任意 点 的 总 势 场 为 斥 力 场 和 引力 场 二 者 释 加 之 和 ,机 器 人 的 受 力 也 是 对 
应 的 引力 和 斥 力 的 又 加 , 即 : 
| UD) = U.. (q) +U (q) 
F(g) =— VU(q) = F, (q) + F,., (q) 
HH MATLAB 绘制 机 器 人 工作 空间 中 的 总 势 场 如 图 7. 13 所 示 , 可 以 看 到 起 点 (5,50) 在 


Vo(qsqas) p(q.,qas) < po 
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势 场 中 较 高 的 “山坡 ”上 ,目标 点 (99,99) 在 势 场 中 的 “山脚 ”处 ,障碍 物 ((30,30)、(50,50)、 
(50,80)) 在 势 场 中 则 是 是 出 的 “山峰 ”。 机 器 人 在 这 种 势 场 中 的 势能 的 引导 下 ,会 避 开 障碍 
物 ,到 达 目 标点 。 


总 势 场 值 


—T TP F 
20 人 


40 e aaa 
60 80 


图 7.13 环境 总 势 场 仿真 


人 工 势 场 法 虽然 简单 实用 ,具有 良好 的 实时 性 ,但 是 它 也 存在 一 些 缺点 ,需要 对 算法 进 
行 改进 。 下 面 列举 三 个 人 工 势 场 法 的 缺点 及 其 对 应 的 改进 方法 。 
问题 1: 在 势 场 中 可 能 存在 引力 和 斥 力 大 小 相等 .方向 相反 的 点 ,机 器 人 经 过 这 样 的 点 
时 可 能 陷入 局 部 陷阱 且 无 法 逃逸 ,该 问题 也 被 称 为 局 部 最 优 解 问题 。 
改进 方案 : 针对 该 问题 ,可 以 通过 判断 机 器 人 所 在 位 置 是 否 是 目标 点 ,如 果 不 是 目标 
点 , 则 加 入 一 随机 扰动 ,让 机 器 人 跳出 局 部 最 优 解 。 
问题 2: 当 目 标点 和 某 个 或 某 些 障 碍 物 距离 很 近 时 , 斥 力 可 能 会 远大 于 引力 ,那么 机 器 
人 将 会 很 难 达 到 目标 点 。 
改进 方案 : 针对 该 问题 ,修改 斥 力 场 公式 如 下 。 在 机 器 人 靠近 目标 时 ,虽然 斥 力 场 要 增 
加 ,但 是 距离 在 减 小 ,由 此 可 在 一 定 条 件 下 减 小 斥 力 场 的 作用 ,增强 引力 场 的 作用 。 
1 1 
U,, (q) = 4 2 Na 
0 p(q+qa s) > po 
问题 3: 当 机 器 人 离 目标 点 比较 远 时 ,引力 相对 较 大 , 则 此 处 相对 较 小 的 斥 力 可 能 被 忽 
略 , 导 致 机 器 人 撞 上 障碍 物 。 
改进 方案 : 针对 该 问题 ,可 修改 引力 场 公式 如 下 ,增加 一 个 距离 浆 值 dioa ,从 而 在 机 器 
人 距离 目标 点 较 远 时 对 其 引力 进行 限制 ,避免 其 过 大 。 


=. ) 0" (qrqdobs) Pl(q»qobs) < po 


1 2 2 
zee (q ?dgoal ) plq ?dgoal ) < ist 
Us (q) = 


| 一 


dgoa £0 (q » qaoal ) — FS dai j PCa» aroa) > d rai 
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7.3 机 器 人 的 轨迹 规划 


路 径 规划 给 出 的 是 机 器 人 运动 的 路 线 ,是 任务 层级 的 机 器 人 运动 。 例 如 ,规划 的 路 径 被 
描述 为 “机 器 人 由 A 点 直线 运动 到 也 点 ”, 路 径 信息 中 不 涉及 运动 速度 .运动 时 间 等 变量 ,是 
纯 几 何 层 面 的 运动 路 径 ,机 器 人 在 执行 规划 路 径 时 可 机 动 调整 ,不 需要 严格 执行 。 而 轨迹 规 
划 生 成 的 是 机 器 人 的 运动 执行 指令 ,是 执行 层级 的 机 器 人 运动 。 规 划 的 轨迹 通常 包含 机 器 
人 的 位 置 .速度 .加 速度 等 信息 ,机 器 人 需要 严格 执行 规划 的 运动 。 机 器 人 的 轨迹 规划 主要 
包括 关节 空间 轨迹 规划 和 操作 空间 轨迹 规划 两 大 类 。 

由 于 不 平稳 的 运动 将 导致 机 器 人 产生 振动 和 冲击 ,使 机 械 零 部 件 的 磨损 和 破坏 加 剧 , 因 
此 进行 机 器 人 的 轨迹 规划 时 要 求 机 器 人 的 运动 轨迹 必须 是 光滑 连续 的 ,而 且 轨 迹 函 数 的 一 
阶 导数 (速度 ) 也 是 光滑 连续 的 ,对 于 一 些 特殊 需求 ,需要 轨迹 函数 的 二 阶 导数 (加 速度 ) 也 是 


7.3.1 关节 空间 的 轨迹 规划 


机 器 人 是 由 多 个 关节 组 成 ,关节 空间 的 轨迹 规划 就 是 对 每 个 关节 都 基于 关节 运动 约束 
条 件 规划 它 的 光滑 运动 轨迹 。 关 节 的 运动 约束 条 件 包 括 它 的 运动 范围 ,运动 速度 ,加 速度 
等 。 例 如 ,在 机 器 人 的 运动 控制 中 ,采用 关节 空间 轨迹 规划 的 流程 如 图 7. 14 所 示 。 首 先 在 
机 器 人 的 操作 空间 中 确定 机 器 人 要 路 过 的 路 径 点 (一 般 称 为 节点 ), 由 运动 学 道 解 方法 求 出 
对 应 的 各 关节 值 ; 针对 每 个 关节 ,在 每 两 个 相 邻 关节 值 之 间 规 划 其 过 滤 运 动 轨迹 ,实际 上 就 
是 采用 光滑 函数 规划 关节 变量 的 平稳 变化 曲线 ,图 7. 15 是 采用 直线 规划 关节 值 之 间 的 过 渡 
轨迹 ; 最 后 让 每 个 关节 在 相同 的 时 间 段 内 执行 完 规划 的 关节 轨迹 , 即 可 实现 预期 的 机 器 人 
在 操作 空间 中 的 运动 。 


确定 路 径 点 一 反 解 关节 值 à 


一 ~ 


关节 执行 ( 运 光滑 函数 拟 
动 时 间 相 同 ) 合 每 个 关节 


图 7.14 关节 轨迹 规划 流程 图 7.15 直线 轨迹 规划 


如 果 给 定 了 关节 起 始点 和 终止 点 的 位 置 变 量 ,使 用 直线 插值 函数 是 规划 关节 轨迹 最 简 
单 的 方式 。 
1. 用 直线 插值 函数 规划 关节 轨迹 
假设 某 关 节 为 旋转 关节 ,起 始 角度 为 & ,终止 角度 为 br ,运动 持续 时 间 为 tj, 可 以 通过 直线 
插值 函数 得 到 关节 运动 的 轨迹 郴 数 。 假 设 关节 从 起 始点 匀速 运动 到 终止 点 ,那么 关节 速度 为 : 
多 二 
ty 


关节 运动 的 轨迹 函数 可 以 表示 为 : 
= 0 +Ü = t 


158 l| 机 器 人 学 一 一 机 构 、 运 动 学 、 动 力学 及 运动 规划 


可 得 到 采用 直线 插值 规划 的 关节 轨迹 函数 通 式 为 : 


0=0 + $ (15 
f 


B| 1: 一 个 旋转 关节 在 3s 内 从 起 始点 bo 一 10* 运 动 到 终止 点 br 二 60 , 求 关 节 的 直线 插 
值 轨迹 函数 。 
解 : 将 0% 王 10 和 br 一 60 "代入 关节 直线 捅 值 的 轨迹 函数 通 式 (7-1) ,可 得 该 关节 的 直线 
插值 轨迹 函数 为 : 
g = 1042, = 10 + 16. 667z 


其 中 ,关节 匀速 运动 的 速度 为 16. 667°/s. 
该 关节 的 运动 轨迹 如 图 7. 16 所 示 , 从 图 中 可 以 看 出 该 关节 的 角度 随时 间 呈 线性 变化 。 
关节 角速度 (关节 轨迹 函数 的 一 阶 导数 ) 曲 线 如 图 7.17 所 示 ,是 一 条 直线 。 关 节 加 速度 的 曲线 
如 图 7. 18 所 示 , 在 起 始点 和 终止 点 处 丝 有 很 大 的 数值 突变 , 即 在 这 两 点 需要 非常 大 的 加 速度 ，。 


0 0.5 1 1.5 2 2:5 3 
tis 


图 7.16 关节 角度 随时 间 变 化 曲线 


öl ys? 


=O 0.5 1 1.5 2 2.3 3 1 
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图 7.17 关节 角速度 随时 间 变 化 曲线 图 7.18 关节 角 加 速度 随时 间 变 化 曲线 


很 显然 ,利用 直线 插值 函数 规划 机 器 人 关节 轨迹 时 存在 一 个 很 大 问题 : 关节 速度 在 节 
点 处 不 连续 ,加 速度 无 限 大 。 因 此 ,在 使 用 线性 插值 函数 规划 关节 轨迹 时 ,为 了 得 到 角度 
和 速度 都 连续 平滑 的 运动 轨迹 ,一般 在 起 始点 和 终止 点 处 增加 一 段 抛物 线 轨迹 的 缓冲 区 
域 , 即 用 线性 轨迹 函数 和 抛物 线 轨迹 函数 组 合 来 规划 机 器 大 的 关节 轨迹 。 由 于 抛物 线 对 
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于 时 间 的 二 阶 导 数 为 常数 ,这 样 就 可 以 在 节点 处 实现 速度 的 平滑 过 渡 , 不 需要 无 穷 大 的 
加 速度 。 

2. 用 抛物 线 过 渡 的 直线 插值 函数 规划 关节 轨迹 

假设 某 关节 为 旋转 关节 ,依旧 给 定 起 始 角 9、 终止 角 0, 以 及 运动 持续 时 间 t;/。 为 了 构 
造 抛物 线 过 渡 的 轨迹 函数 ,假设 两 端的 抛物 线 过 渡 域 的 持续 时 间 ts 相同 ,因而 在 这 两 个 过 
渡 域 中 采用 大 小 相等 .符号 相反 的 加 速度 值 , 这 种 构造 具有 多 个 解 ,得 到 的 轨迹 也 不 是 唯一 
的 ,但 是 每 一 个 解 都 对 称 于 时 间 中 点 z, 和 位 置 中 点 9, ,如 图 7. 19 所 示 。 

由 于 过 渡 域 L0 ,zj 终点 的 速度 必须 等 于 线性 
域 的 速度 ,所 以 关节 在 时 刻 的 速度 为 : 
0, — Os _ 0, — 0, 


C =2) 


其 中 0, 为 过 渡 域 终点 4 的 关节 角度 。 
用 0 表示 过 渡 域 内 的 加 速度 ,0, 可 表示 为 : 


O tp 加 fet ty 


图 7.19 带 抛 物 线 过 渡 的 线性 插值 示意 图 


0, = 0 + + (7-3) 
> —_ _ 0, +0; py R 
由 于 Å, 一 0 "如 ,0, 二 一 ,综合 式 (7-2) 和 式 (7-3) 可 得 : 
Gt — trta + (0;—0)=0 (7-4) 


这 样 ,对 于 任意 给 定 的 起 始 角 0, .终止 角 0, ARZ EE t, ONER 7-4) 
择 相 应 的 过 渡 域 加 速度 9 和 过 渡 域 时 间 ,得 到 对 应 的 抛物 线 轨迹 。 通 常 的 作法 是 先 选 择 
加 速度 9 的 值 ,然后 求解 6: 


t NOE — 40(0;— 0,) 


2 


b 


20 
为 了 保证 t, 有 解 ,过 渡 域 加 速度 值 9 必须 选 得 足够 大 , 即 : 


Ë 
可 以 发 现 , 当 上 述 不 等 式 中 等 号 成 立时 ,线性 域 的 长 度 缩减 为 零 , 整 个 路 径 段 有 两 个 过 


渡 域 组 成 ,其 连接 处 速度 相等 ; 相反 ,0 的 取 值 越 大 ,过 渡 域 的 长 度 越 短 , 当 9 的 取 值 为 无 限 
大 时 ,过 渡 域 长 度 变 为 零 ,机 器 人 运动 轨迹 又 变 成 了 直线 。 


在 得 到 0 # t, 之 后 ,可 以 依次 得 到 9, 和 0, ,并 据 此 得 到 关节 轨迹 的 分 段 函数 : 


Ó +Ó LiL h 
K= gh + göt + ËL t spam t == (7-5) 
— 1 k == y saw Pus Éy 
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例 2: 一 个 旋转 关节 在 3s 内 从 起 始点 0, 二 10" 运 动 到 终止 点 90/ 二 60", 求 关节 的 抛物 线 
过 渡 的 线性 插值 轨迹 函数 。 


解 : 首先 选取 过 渡 域 的 加 速度 0 ,其 应 满足 ; 


4(0, — h) rs 200° _ ° 
i 


` 


0 > 


选取 0=30°,j3R 8 ;,=0.7362s, I| 48 X: 35 BJ 8 jp RK : 


ddd 


| 


lot 152 
1. 8702 + 22. 0860: 
— 15 F 90£— 75 


0 = +< 0. 7362 
0. 7362 < t < 2. 2638 
2. 2638 rS: 3 


3s 内 关节 的 角度 变化 如 图 7. 20 所 示 。 可 以 看 出 关节 的 角度 变化 是 光滑 的 。 关 节 的 角 
速度 和 角 加 速度 的 变化 分 别 如 图 7. 21 和 图 7. 22 所 示 。 关 节 的 角速度 曲线 呈 梯 形 ,由 加 速 
段 、 匀 速 段 和 减速 段 组 成 ,属于 等 加 速 等 减速 运动 。 关 节 的 加 速度 曲线 是 由 多 个 线段 组 成 ， 
变化 不 光滑 ,存在 加 速度 瞬时 要 达到 某 个 值 及 瞬时 又 要 变 为 零 的 突变 ,这 会 造成 关节 的 振动 
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图 7.20 关节 角度 随时 间 变 化 
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图 7.21 关节 角速度 随时 间 变 化 


ts 


图 7.22 关节 角 加 速度 随时 间 变 化 
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由 于 加 速度 是 可 以 任意 选取 的 ,所 以 采用 抛物 线 过 渡 的 直线 插值 函数 规划 机 器 人 关节 
的 运动 轨迹 有 无 穷 多 条 。 

3. 用 三 次 多 项 式 函 数 规划 关节 轨迹 

三 次 多 项 式 函数 具有 一 阶 、 二 阶 微 分 光滑 特性 ,所 以 在 机 器 人 关节 空间 轨迹 规划 中 被 普 
遍 采 用 。 

假设 某 机 器 人 关节 运动 过 程 中 需要 通过 五 个 关节 位 置 , 如 图 7. 23 所 示 , 在 这 五 个 关节 
值 中 ,任意 两 个 相 邻 的 关节 值 都 需要 做 轨迹 规划 ,所 有 的 规划 轨迹 连 起 来 即 可 构成 该 关节 的 
运动 轨迹 或 运动 曲线 。 定 义 一 对 关节 值 中 的 起 始点 为 , 2 
止 点 为 0 ,轨迹 规划 的 任务 就 是 构造 出 满足 关节 运动 约束 条 
件 且 通过 起 始点 和 终止 点 的 光滑 轨迹 函数 90(1)。 下 面 以 
图 7. 23 中 起 始 的 两 个 关节 值 为 对 象 ,介绍 如 何 采 用 三 次 多 项 
式 函 数 规划 关节 运动 轨迹 。 


t 


三 次 多 项 式 函 数 的 通 式 为 : 图 7.23 三 次 多 项 式 函 数 规划 
0(t) = ao + ait + a, + azt? (7-6) 关节 轨迹 
为 了 实现 关节 的 平稳 运动 ,轨迹 函数 Ci) 至少 需 要 满足 四 个 约束 条 件 ,起 始点 和 终止 点 
的 角度 约束 和 速度 约束 。 
角度 约束 : 
DCLr) 一 0 

速度 约束 : 

uo 

Ee 


将 四 个 约束 条 件 代 入 三 次 多 项 式 函数 通 式 (7-6) ,可 得 到 下 面 四 个 方程 : 


ba = Aa 
0; = ao Fait Hat +a, 


EA 
求解 可 得 三 次 多 项 式 的 系数 为 : 


âo = bo 
w = È, 
azə 和 O; 0) 22, F} (Y=79 


a3 一 名 (0 一 做 站 +Ó, +Å) 
f 


将 上 述 系数 代入 三 次 多 项 式 通 式 , 即 可 得 这 两 个 关节 点 之 间 的 轨迹 函数 0(z) 。 
例 3: 一 个 旋转 关节 在 3s 内 从 起 始点 0, = 10" 运 动 到 终止 点 0, 一 60", 起 始 的 速度 为 
1 Vs, 终止 的 速度 为 2 /s, 求 关节 的 三 次 多 项 式 轨迹 函数 0(z) 。 
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解 : 由 三 次 多 项 式 的 系数 求解 公式 (7-7) ,可 得 各 系数 为 : 
= `Q š: 


a, = 1 


< a2 i (0; Q) — 15. 33 


as š (0, bo) = 9. 37 


f 


将 上 述 系 数 代 入 三 次 多 项 式 通 式 (7-6) ,可 得 关节 的 三 次 多 项 式 轨迹 函数 为 : 
OC) = 10 + t+ 15. 33 — 3. 372° 


该 关节 在 3s 内 的 运动 轨迹 如 图 7. 24 所 示 。 从 图 中 可 以 看 出 ,该 关节 在 这 3s 内 的 角度 
变化 是 光滑 的 。 


0 0.5 I 1.5 2 25 3 
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图 7. 24 关节 运动 角度 随时 间 变 化 


该 轨迹 函数 对 应 的 一 阶 和 二 阶 导 数 分 别 为 : 
| 1 十 30.67+ 一 10.112 
BC) = 30. 67 — 20. 22; 


它们 分 别 对 应 了 在 这 3s 内 该 关节 的 速度 和 加 速度 变化 情况 ,如 图 7.25 和 图 7.26 所 
示 。 这 两 个 图 表明 ,在 这 3s 内 ,该 关节 的 角速度 和 和 角 加 速度 都 是 光滑 变化 的 。 


25 让 T T | 
30 J 

20 
20 村 
Ta 15 A 10 | 
5 10 号 OF ] 
-10 d 

5 
-50 1 
00 0.5 l 1.5 2 2.5 3 -300 0.5 1 1.5 2 2.5 3 

tls tls 


图 7.25 关节 运动 角速度 随时 间 变 化 图 图 7.26 关节 运动 角 加 速度 随时 间 变 化 
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故 可 得 出 结论 : 采用 三 次 多 项 式 函 数 的 关节 速度 曲线 为 抛物 线 ,加 速度 曲线 为 直线 。 

从 上 面 的 例子 可 以 发 现 ,采用 三 次 多 项 式 的 关节 轨迹 函数 只 能 保证 两 关节 点 处 的 位 置 
和 速度 约束 ,无 法 满足 两 关节 点 处 的 加 速度 约束 。 如 果 想 要 满足 加 速度 约束 ,需要 采用 更 高 
阶 的 多 项 式 函 数 , 如 五 次 多 项 式 函 数 。 

4. 用 五 次 多 项 式 函 数 规划 关节 轨迹 


五 次 多 项 式 函 数 的 通 式 为 : 
Ot) = a Hait +a, + a, + a,t' + as t° (7-8) 
轨迹 函数 90(z) 需 要 满足 六 个 约束 条 件 , 起 始点 和 终止 点 的 角度 约束 、 速 度 约束 和 加 速度 
约束 。 
角度 约束 : 
Olt) =0; 
速度 约束 : 
"mga 
Ê Ct) =À, 
加 速度 约束 : 


plt =i; 


将 上 述 6 个 约束 条 件 代 和 人 五 次 多 项 式 通 式 , 可 得 6 个 方程 : 


b 一 ao 

br = as Harts Hart tasty a Hast} 
Ê = a 

10 = a) + 2axty + 3as, T+4a, + 5ast' 
bo = 2az 


0; = Za + bast, + 12a, t + 20a;t 
求解 上 述 方程 组 ,可 得 五 次 多 项 式 的 系数 为 : 


ao = bo 

而 =f 

„aË 

a = 
_ 20(0, — 0) — (80, + 128 )t; — (30, — 0DE 

Jä; = BEGLAR a ERRI 

_ 30(0, — 0) + (140, + 168 )tr + (30, — 20t 

ai. pp EPE 

a — 120, — 05 — (6É, + Gået — (0 — 9) 

: 


f 
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将 上 述 系数 代入 五 次 多 项 式 通 式 , 即 可 得 这 两 个 关节 点 之 间 的 轨迹 函数 b(z)。 
例 4: 一 个 旋转 关节 在 3s 内 从 起 始点 如 =10 运动 到 终止 点 6, = 60° ,起 始 的 速度 为 
1 Vs, 终 止 的 速度 为 2 1s, 起 始 的 加 速度 为 2 /s ,终止 的 加 速度 为 4/s , 求 关节 的 五 次 多 项 


式 轨迹 函数 0(z) 。 
解 : 由 五 次 多 项 式 的 系数 求解 公式 (7-9) ,可 得 各 系数 为 : 
a = 10 
a, = 1 
Qa = 1 
as = = 16.630 
A =- == 556 


= 94 = 
üs = 81 i 160 


将 上 述 系数 代入 五 次 多 项 式 通 式 (7-8) ,可 得 关节 的 五 次 多 项 式 轨迹 函数 为 : 
0Gz) = 10 + t + # + 16. 63088 — 8. 556# + 1.160 
该 关节 在 3s 内 的 运动 轨迹 如 图 7. 27 所 示 。 从 图 中 可 以 看 出 : 该 关节 在 这 3s 内 的 角度 
变化 是 光滑 的 。 


0 0.5 l 15 2 25 3 
tis 


图 7.27 关节 运动 角度 随时 间 变 化 


该 轨迹 函数 对 应 的 一 阶 和 二 阶 导数 分 别 为 : 


ÖCE) = 1 二 2t 49. 8892 — 34. 2248 十 5. 800z! 


0(t) = 2 + 99. 778; — 102. 667 + 23. 210 
它们 分 别 对 应 了 在 3s 内 该 关节 的 速度 和 加 速度 变化 情况 ,如 图 7. 28 和 图 7. 29 所 示 。 
这 两 个 图 表明 ,在 这 3s 内 ,该 关节 的 角速度 和 角 加 速度 都 是 光滑 变化 的 。 
由 此 可 得 出 结论 : 采用 五 次 多 项 式 函 数 的 关节 速度 曲线 为 抛物 线 , 加 速度 曲线 为 正弦 曲线 。 
机 器 人 关节 空间 轨迹 规划 的 优点 是 计算 简单 .无 奇异 性 ,缺点 是 操作 空间 轨迹 可 能 不 光 
滑 。 由 于 关节 空间 到 操作 空间 的 映射 是 非 线 性 的 ,所 以 关节 空间 中 轨迹 的 光滑 性 不 代表 着 
操作 空间 中 对 应 的 轨迹 也 是 光滑 的 。 


第 7 章 ”机 器 人 的 运动 规划 P> 165 


L m. 
0 0.5 l 15 2 25 3 
tls 


图 7.28 关节 运动 角速度 随时 间 变 化 图 7.29 关节 运动 角 加 速度 随时 间 变 化 


7.3.2 操作 空间 的 轨迹 规划 


机 器 人 的 操作 空间 也 就 是 机 器 人 的 工作 空间 。 

在 机 器 人 的 关节 空间 中 规划 其 关节 运动 轨迹 ,可 保证 机 器 人 的 末端 经 过 起 点 和 终点 ,但 
在 两 点 之 间 的 轨迹 是 未 知 的 , 它 依 赖 于 每 个 机 器 人 独特 的 运动 学 特性 。 在 很 多 应 用 中 需要 保 
证 机 器 人 未 端的 运动 轨迹 ,比如 弧 焊 ,这 就 需要 在 机 器 人 的 操作 空间 中 规划 机 器 人 的 轨迹 。 

操作 空间 的 轨迹 规划 一 般 是 在 笛 卡 儿 直 角 坐 标 系 中 进行 的 。 规 划 出 机 器 人 操作 空间 的 
轨迹 后 ,通过 循环 对 机 器 人 的 末端 定位 点 反 解 求 出 各 关节 值 , 即 可 控制 机 器 人 实现 预期 的 操 
作 空 间 运动 轨迹 。 在 笛 卡 儿 坐 标 系 中 进行 轨迹 规划 具有 下 列 优点 : 

(1) 机 器 人 的 运动 轨迹 是 直观 的 ,容易 理解 和 描述 ; 

(2) 第 卡 儿 坐 标 系 中 的 机 器 人 运动 可 以 非常 容易 地 推广 到 圆柱 坐标 系 、 球 坐标 系 以 及 
其 他 正 交 坐标 系统 。 

在 笛 卡 儿 坐 标 系 中 ,机 器 人 末端 的 轨迹 可 用 一 系列 的 “节点 "来 表示 。 所 谓 的 节点 就 是 
机 器 人 操作 空间 轨迹 上 的 “拐点 ?或 关键 点 ,可 用 机 器 人 末端 坐标 系 相 对 于 参考 系 的 位 姿 来 
表示 。 下 面 看 一 个 机 器 人 装配 销 钉 的 运动 轨迹 ,如 图 7. 30 所 示 , 机 器 人 末端 抓 手 的 运动 轨迹 
是 在 笛 卡 儿 坐 标 系 中 描述 的 。 该 机 器 人 运动 轨迹 中 的 节点 及 对 应 的 动作 说 明 如 表 7. 1 所 示 。 


图 7.30 机 器 人 装配 销 钉 的 运动 轨迹 
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表 7.1 机 器 人 装配 销 钉 的 节点 及 说 明 


序 号 节点 节 疲 说 明 

1 | P, 抓 手 初始 位 

2 P, 接近 销 钉 位 
a 

3 P, 销 钉 的 位 置 

4 P. 提起 销 钉 位 
A ES 

5 P, 接近 销 钉 孔 

6 P. 插入 销 钉 位 

7 P, 离开 销 钉 孔 


如 果 采 用 直线 连接 各 节点 ,可 得 机 器 人 的 运动 轨迹 如 图 7. 31 所 示 。 为 了 让 机 器 人 按 
此 轨迹 运动 ,需要 规划 两 节点 之 间 的 机 器 人 运动 轨迹 。 机 器 人 操作 空间 轨迹 规划 的 核心 
问题 就 是 如 何在 相 邻 的 两 个 节点 之 间 生 成 一 系列 的 中 间 点 ,所 以 轨迹 规划 也 被 称 为 轨迹 
插 补 。 

1. 操作 空间 的 直线 轨迹 规划 

在 机 器 人 的 操作 空间 中 ,机 器 人 末端 轨迹 上 的 每 个 节点 包含 了 三 个 位 置 变量 和 三 个 姿 
态 变 量 。 三 个 位 置 变量 是 用 该 点 的 笛 卡 儿 坐 标 表 示 , 如 zx,y,z<。 对 于 机 器 人 的 姿态 ,前面 介 
绍 了 用 旋转 矩阵 表示 机 器 人 姿态 的 方法 ,但 是 对 于 两 个 节点 的 姿态 矩阵 ,是 无 法 在 中 间 线 性 
插入 一 个 或 几 个 中 间 状 态 的 姿态 矩阵 的 ,因此 在 操作 空间 的 直线 轨迹 规划 中 ,机 器 人 的 姿态 
不 能 用 旋转 矩阵 来 表示 ,可 以 采用 三 变量 的 RPY 角 或 欧 拉 角 表 示 机 器 人 的 姿态 。 

假设 采用 ZYX 欧 拉 角 表 示 机 器 人 末端 在 操作 空间 中 的 姿态 , 设 机 器 人 操作 空间 中 的 
两 个 节点 分 别 为 P 和 P, , P, = (x, »Y1 121 201 D V1) P, = (Zs > yz +Z ,a2 ,7a) 。 则 机 器 人 
操作 空间 的 直线 轨迹 规划 / 插 补 方法 如 下 : 

(1) 确定 机 器 人 末端 运动 的 速度 v、 加 速度 a、 插 补 时 间 1。 插 补 时 间 是 由 机 器 人 的 伺服 
控制 周期 决定 的 ,通常 为 ms 级 ,如 10ms .20ms, 

(2) 计算 两 节点 之 间 的 直线 距离 为 : 

A 

(3) 计算 插 补 次 数 , 步 长 AS vt, MERKRA 
j L 
a 六 为 整数 

int (六 )+1， + 非 整数 
(4) 计算 机 器 人 末端 的 运动 变量 为 : 
dp = [E n, n n n ea, Ë A nT 


= , 
n n n n n 


(5) 由 公式 pi+1 王 Pr 十 dp,k 二 1,2,…n, 即 可 求 出 机 器 人 末端 在 下 一 插 补 点 的 位 姿 , 采 
用 运动 学 道 解 算法 求 出 对 应 的 关节 变量 值 w+i。 

(6) 把 每 次 计算 出 的 关节 变量 值 w+: 按 周期 发 送 给 机 器 人 的 伺服 控制 器 , 即 可 控制 机 
器 人 未 端 按 直线 轨迹 运动 , 依 此 循环 直到 完成 所 有 的 插 补 。 
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机 器 人 操作 空间 的 直线 轨迹 插 补 算法 的 流程 如 图 7. 31 所 示 。 


计算 两 节点 之 间 
的 直线 距离 


计算 插 补 次 数 


计算 机 器 人 末端 的 
运动 变量 


求解 关节 变量 


发 送 给 伺服 控制 器 


图 7.31 直线 轨迹 插 补 流程 


例 5: 如 图 7.32 所 示 是 一 个 8 自由 度 的 模块 机 器 人 安装 在 一 个 1 自由 度 的 移动 导轨 
上 。 设 机 器 人 末端 起 始 节点 位 置 和 姿态 为 P, =(0,110,1555,90,0,0) , 终 节 点 位 置 和 姿态 
为 P, 二 (1260,280,1445,45,45,90), 采 用 ZYZ 欧 拉 角 表 示 姿 态 。 对 这 两 节点 进行 直线 运 
动 轨迹 规划 。 


图 7.32 一 个 带 移动 导轨 的 9 自由 度 机 器 人 


解 : 

机 器 人 参数 设置 如 下 : 

(1) 末端 速度 : 0.2m/s; 

(2) 末端 加 速度 : 0. 4my/s ; 

(3) 插 补 时 间 : 0. 02s。 

计算 可 得 两 节点 距离 1 二 1148. 8mm, 总 运动 时 间 :一 9.74s, 插 值 点 数 n= 487. 

由 机 器 人 末端 的 运动 变量 dp, 采 用 道 解 算法 可 求 得 对 应 的 关节 变量 增 量 dg, ,由 
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qeri = qe F dqr BIAR H AAI KT E o AE ARA 
动 到 新 位 置 ,循环 插 补 直到 487 次 插 补 全 部 完成 。 
末端 的 运动 变量 一 般 不 是 dp ,而 是 比 dp 小 的 值 

该 9 自由 度 机 器 人 末端 直线 运动 轨迹 的 仿真 如 图 7 
的 初始 位 姿 和 期 望 轨迹 ,图 7.33(b) 显 示 了 机 咒 
迹 。 仿 真 结 果 表 明 ,该 直线 运动 轨 
光滑 直线 运动 


合 机 


和 


(a) 初始 位 姿 和 期 望 娄 迹 


图 7.33 9 自由 度 机 器 人 末端 直线 运动 轨迹 
直线 轨迹 规划 是 机 器 人 操作 空间 轨迹 规划 中 最 简单 、 最 党 | 
迹 规划 等 。 


2. 操作 空间 的 圆 弧 轨迹 规划 

在 机 器 人 的 操作 空间 中 ,规划 机 器 人 的 直线 轨迹 只 
轨迹 需要 三 个 节点 。 

(1) 由 机 器 人 


目的 一 种 ,复杂 的 还 有 圆 弧 轨 


1 器 人 控制 器 即 可 控制 机 器 人 末端 运 
需要 注意 的 是 


,最 后 一 次 捅 补 时 ,机 器 人 


.33 所 示 , 图 7. 


33(a) 显示 了 机 器 人 
fY 人 的 最 终 位 姿 和 机 器 人 未 端 走 过 的 实际 轨 
迹 规 划 方 法 实现 了 机 器 人 从 初始 位 姿 到 最 终 位 姿 之 间 的 


(b) 终止 位 次 和 实际 轨迹 


“uka 点 ,但 规划 机 器 人 的 圆 弧 


机 器 人 操作 空间 的 圆 弧 轨迹 规划 J 分 y 
潜 作 空间 中 的 三 个 节点 求 圆 弧 轨迹 的 圆心 和 半径 

假设 机 器 人 操作 空间 中 的 三 个 节点 分 别 是 圆 弧 的 起 点 
P, 坐 标 为 (zj ;yj,:z,); 圆 弧 的 中 间 点 Q. MERN (>, + 
圆 弧 的 终点 及 ,坐标 为 (z,， 


yrs z) s EIR KI EO A C 


PAR TAT 
,坐标 为 


Czeyyeyz), 圆 弧 半 径 为 ,参考 坐标 系 为 1OXYZ} ,如 图 7. 34 
所 示 。 
向 量 PQ 和 QR 可 分 别 表 示 为 : 
= 
图 7.34 空间 三 点 及 相关 参量 QR = [z,— Z, y,— Lr] 
则 PQ、R 三 点 所 在 平面 1 的 法 问 量 n 为 : 
i J k 
n= ln: fy m] == PO QR = |z, ==; y¿-— y, m=" Ëy 
一 


由 圆 的 性 质 可 知 ,线段 PQ 的 中 垂 线 和 线段 QR 的 中 垂 线 的 交点 即 为 


面 分 别 求 线段 PQ 和 QR 的 中 垂 线 所 在 的 直线 方程 


圆 弧 的 圆心 。 下 
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由 于 同时 垂直 于 向 量 n 和 向 量 PQ 的 向 量 即 为 线段 PQ 的 中 垂 线 的 方向 向 量 n, , 则 : 
i z k 


n = Lm: ns; nri =rX PO == n; n, n. 


Za Te Ys Yp Za Z 


假设 线段 PQ 的 中 点 为 A ,其 坐标 为 (a; ,a,,a:), 由 卫 点 和 Q 点 的 坐标 可 得 : 


a, = atte, a, = 4t, a, =t 
则 线段 PQ 的 中 垂 线 所 在 直线 的 方程 为 : 
mz—a, _ y—a z—a 
Nir 34 niy k: ni: 
同 理 , 同 时 垂直 于 向 量 n 和 向 量 QR 的 向 量 即 为 线段 QR 的 中 垂 线 的 方向 向 量 n;, 则 : 
i j k 
n = [ns na na T = # > QR = n, ny fiz 
Tr ~ Ry Ye Ve, Te Xy 


假设 线段 QR 的 中 点 为 B, 其 坐标 为 (b,,b,,65.), 由 QQ 点 和 R 点 的 坐标 可 求 得 : 
Et M i= bs -aTr 
b, = Dro b, = 人 b. — 5 


则 线段 QR 的 中 垂 线 所 在 直线 的 方程 二: 为 : 
e= b; Ll z G 
Nor no 712> 
由 圆 的 性 质 可 知 ,直线 工 ! 和 上 ;的 交点 即 为 圆 弧 轨迹 的 圆心 。 求 解 两 条 空间 直线 的 交点 
的 方法 比较 多 ,如 参数 方程 的 方法 和 线性 方程 组 的 方法 等 。 这 里 采用 线性 方程 组 解法 ,将 直 
线 方程 L, 和 工 ,转化 成 空间 直线 的 一 般 式 ,由 于 方程 组 个 数 大 于 未 知 量 个 数 , 故 利用 广义 北 
来 求 Ll 和 工 ,的 交点 , 即 圆 弧 的 圆心 坐标 为 : 


si 
fy fis © aiy — näy 
Te 
0 ni: Niy a,ni., — a,.niy, 
Ye = . 
nzy Naz 0 binzy 一 nx;b y 
Ze 
0 722= no byna: = b.n;, 


求 得 了 圆 弧 的 圆心 坐标 (xz.,y.,z.), 则 CP ..CQ.CR 三 个 向 量 的 模 都 等 于 圆 弧 的 半径 +， 

WA: 
r= |CP|= |@|= |@| 

(2) 将 空间 圆 弧 转换 为 平面 圆 弧 

如 图 7.35 所 示 , 建 立 坐 标 系 {0O'X’Y’Z') ,使 原点 O 
位 于 圆心 C 处 ,以 向 量 CP 所 在 直线 为 X' 轴 ,方向 与 向 量 
CP 相同 ,过 点 C 且 垂直 于 平面 1 的 直线 为 Z' 轴 ,Y 轴 由 右 
FEMME, EFH XOY 与 平面 1 重合 。 

假设 X 轴 上 的 单位 向 量 为 站 , 它 在 坐标 系 {OXYZ} 中 
可 表示 为 : ii 


i 二 K: y, Ye maj 
CP CP cp | 图 7.35 平面 坐标 系 定义 
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假设 Z 轴 上 的 单位 向 量 为 上 , 它 在 坐标 系 {1OXY2Z ) 中 可 表示 为 : 
= n = [e e. aa. 
Jn] Lla] Jn] In] 
假设 冯 轴 上 的 单位 向 量 为 六 , 它 在 坐标 系 {1OXYZ } 中 可 表示 为 : 
=k x i 


Piet ttet t te t tet Pi 


TX, Te My _ [Xe Ne n, i 
MIRA (OXYZ } 到 坐标 系 {OXYZ} 之 间 的 转换 矩阵 *T H: 
De = N; n. P kaqta n, Iy 
R TOP] Tal 。 leeT Tal “Tx. ° 
y — 3 Z, — Zc n; <s e n; n, 
meller e Tal fapT "Tal Tal > 
E i SEP = Su n, Ws = Sie n; fl; 
Te ae] m ea Tal A 
0 0 0 T] 
已 知 坐标 系 {OX’Y'Z'} 中 任意 点 的 坐标 (x ,y ,zx ), 则 它 在 坐标 系 {OXYZ) 中 对 应 点 
的 坐标 (x,y,z) 可 由 下 式 求 得 : 


= R < R 


同样 ,已 知 坐标 系 {OXYZ) 中 任意 点 的 坐标 (zx,y,z) , 则 该 点 在 坐标 系 {O'X'Y'Z'} 中 对 
应 点 的 坐标 (x ,y ,zx ) 可 由 下 式 求 得 : 


F J 
Jy 
z 
1 1 
利用 上 式 就 可 以 求 得 P.Q.R.C 四 个 点 在 坐标 系 {O'X'Y’Z'} 中 的 坐标 。 
(3) 圆 弧 轨 迹 规 划 
圆 弧 轨 迹 规 划 首 先 在 平面 XO'Y’ 中 进行 ,计算 出 平 


面 圆 弧 轨迹 上 的 插 补 点 在 坐标 系 {0O'X'Y'Z"}) 中 的 坐标 ， 
然后 通过 坐标 变换 求 出 该 点 在 参考 坐标 系 {OXYZ} 中 的 


坐标 ,具体 步骤 如 下 。 
首先 ,在 平面 坐标 系 {X'O'Y’} 中 ,计算 圆 弧 PQR 的 
圆心 角 0 和 插 补 次 数 n, 


如 图 7.36 所 示 , 圆 心 角 9 可 能 有 两 种 情况 :0 二 x( 小 
AIL PQR) 和 0 二 x( 大 圆 弧 PQR)。 这 里 ,利用 向 量 O P 
和 PR 的 法 向 量 n; 与 平面 XO'Y 的 法 向 量 的 内 积 来 判 


7.36 圆心 角 可 能 情况 
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断 9 的 值 。 
H=n;.n=OPXPR.n 


# 昌之 0, 则 90<x,0= 2arcsin( 5R| ) 


# H<0, W 0>x,0=2n—2arcsin( LER| ). 


假设 机 器 人 的 插 补 周期 为 1, 移动 速度 为 v, 则 机 器 人 末端 执行 器 从 当前 点 切 向 移动 到 
下 一 个 插 补 点 的 距离 为 : 
As = vt 
则 步 距 角 6 可 近似 表示 为 : 
ô = arcsin(ut /r) 
则 揪 补 次 数 ”为 : 
n=[0/6 | 
然后 ,在 坐标 系 {O'X’Y’Z'} 中 ,从 PP 点 开始 ,依次 计算 出 圆 弧 轨迹 上 每 个 插 补 点 的 位 置 
坐标 (zx’,y ,0) ,再 利用 坐标 变换 矩阵 工 求 得 该 点 在 机 器 人 参考 坐标 系 {OXYZ } 中 的 坐标 
(zy,z), 这 样 就 完成 了 机 器 人 圆 弧 轨迹 的 规划 。 将 各 点 坐标 发 送 给 机 器 人 控制 器 , 即 可 控 
制 机 器 人 末端 按 顺 序 依次 经 过 各 插 补 点 ,从 而 完成 圆 弧 轨迹 运动 。 
例 6: 已 知 机 器 人 操作 空间 中 的 三 个 节点 A、B、C, 起 点 A 的 坐标 为 (100, 0, 0) ,中 间 点 
B 的 坐标 为 (0, 0, 100) ,终点 C 的 坐标 为 (0, 100, 0) ,单位 为 mm, 如 图 7. 37 所 示 。 规 划 机 
器 人 通过 这 三 个 点 的 圆 弧 轨迹 。 


图 7.37 空间 三 点 示意 图 


解 : 

机 器 人 参数 设置 如 下 : 

(1) 末端 速度 v 一 163. 2mm/s; 

(2) 插 补 时 间 t=0. 02s。 

依据 上 面 介 绍 的 圆 弧 轨迹 规划 方法 ,首先 计算 得 到 A、B、C 三 点 构成 圆 弧 的 圆心 坐标 为 
(33.3, 33.3, 33. 3) , 圆 弧 半 径 r= 二 81. 6mm, 圆 弧 夹 角 0 二 240", 步 距 角 6 二 2. 3° ,总 运行 时 间 
T=2. 1s, 插 补 次 数 n 王 105。 

然后 ,利用 圆 弧 轨迹 规划 算法 计算 得 到 所 有 插 补 点 在 坐标 系 {O'X'Y'Z'} 中 的 坐标 ,再 
将 其 转换 到 坐标 系 {OXYZ} 中 , 即 可 得 到 该 圆 弧 所 有 插 补 点 的 三 维 空间 坐标 。 利 用 
MATLAB 画 出 该 圆 弧 轨迹 如 图 7. 38 所 示 。 
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图 7.38 规划 的 圆 弧 轨 迹 
7.4 小结 


本 章 主要 介绍 了 机 器 人 运动 规划 的 两 个 主要 分 支 : 路 径 规划 和 轨迹 规划 。 在 路 径 规划 
部 分 主要 介绍 了 栅 格 法 的 全 局 路 径 规划 方法 和 势 场 法 的 局 部 路 径 规划 方法 。 在 轨迹 规划 部 
分 主要 介绍 了 关节 空间 的 轨迹 规划 方法 和 操作 空间 的 轨迹 规划 方法 ,在 关节 空间 的 轨迹 规 
划 方 法 中 主要 介绍 了 直线 插值 三 次 多 项 式 插 值 、 五 次 多 项 式 插值 的 轨迹 规划 方法 ,在 操作 
空间 的 轨迹 规划 方法 中 主要 介绍 了 直线 轨迹 规划 和 圆 弧 轨 迹 规划 方法 。 
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